diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..523e909 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "mjsim_quad"] + path = mjsim_quad + url = https://github.com/nimesh00/mjsim_quad.git diff --git a/locomotion/CMakeLists.txt b/locomotion/CMakeLists.txt index 1f3877a..a64f163 100644 --- a/locomotion/CMakeLists.txt +++ b/locomotion/CMakeLists.txt @@ -22,4 +22,4 @@ add_subdirectory(src/utils) add_subdirectory(src/dynamics) add_subdirectory(src/communication) add_subdirectory(src/execution) -add_subdirectory(src/robot_simulation) \ No newline at end of file +# add_subdirectory(src/robot_simulation) \ No newline at end of file diff --git a/locomotion/src/communication/CMakeLists.txt b/locomotion/src/communication/CMakeLists.txt deleted file mode 100644 index 69ad02c..0000000 --- a/locomotion/src/communication/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - -include_directories( - include - /opt/ros/foxy/include -) - -install(DIRECTORY include/ - DESTINATION ${COMMON_INCLUDE_DIR} - FILES_MATCHING PATTERN "*.h*" -) - -add_library(quad_communication - src/CommunicationManager.cpp - src/SHM.cpp -) -install(TARGETS quad_communication - ARCHIVE DESTINATION ${COMMON_LIBRARY_DIR} - LIBRARY DESTINATION ${COMMON_LIBRARY_DIR} -) - -add_library(ros_comm - src/QuadROSComm.cpp -) -target_link_libraries(ros_comm - ${rclcpp_LIBRARIES} - ${sensor_msgs_LIBRARIES} - ${nav_msgs_LIBRARIES} - quad_communication -) -install(TARGETS - ros_comm - ARCHIVE DESTINATION ${COMMON_LIBRARY_DIR} - LIBRARY DESTINATION ${COMMON_LIBRARY_DIR} -) - -add_executable(test_ros_comm - tests/test_ros_comm.cpp -) -target_link_libraries(test_ros_comm - ros_comm -) -install(TARGETS - test_ros_comm - ARCHIVE DESTINATION ${COMMON_INSTALL_DIR} - RUNTIME DESTINATION ${COMMON_INSTALL_DIR} -) \ No newline at end of file diff --git a/locomotion/src/communication/src/CommunicationManager.cpp b/locomotion/src/communication/src/CommunicationManager.cpp deleted file mode 100644 index 9838efe..0000000 --- a/locomotion/src/communication/src/CommunicationManager.cpp +++ /dev/null @@ -1,53 +0,0 @@ -#include "CommunicationManager.hpp" - -CommunicationManager::CommunicationManager(const DATA_ACCESS_MODE& mode) : m_name("svan"), m_mode(mode) { -} - -CommunicationManager::CommunicationManager(const std::string& name, const DATA_ACCESS_MODE& mode) : m_name(name), m_mode(mode) { -} - -void CommunicationManager::writeSensorData(const QuadrupedSensorData& sensor_data) { - if (m_mode == DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) { - std::cout << "Access mode set to only read sensor data. Cannot Write. Operation failed.\n"; - return; - } - if (m_sensor_data_ptr == NULL) { - std::cout << "Could not get the address to write sensor data...\n"; - return; - } - m_sensor_data_ptr->copy(sensor_data); -} - -void CommunicationManager::writeCommandData(const QuadrupedCommandData& cmd_data) { - if (m_mode == DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) { - std::cout << "Access mode set to only read command data. Cannot Write. Operation failed.\n"; - return; - } - if (m_joint_command_data_ptr == NULL) { - std::cout << "Could not get the address to write command data...\n"; - return; - } - m_joint_command_data_ptr->copy(cmd_data); -} - -void CommunicationManager::getSensorData(QuadrupedSensorData& sensor_data) { - if (m_mode == DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) { - std::cout << "Access mode set to only write sensor data. Cannot read. Operation failed.\n"; - return; - } - if (m_sensor_data_ptr == NULL) { - std::cout << "Memory not initialized. Cannot read.\n"; - } - sensor_data.copy(*m_sensor_data_ptr); -} - -void CommunicationManager::getCommandData(QuadrupedCommandData& cmd_data) { - if (m_mode == DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) { - std::cout << "Access mode set to only write command data. Cannot read. Operation failed.\n"; - return; - } - if (m_joint_command_data_ptr == NULL) { - std::cout << "Memory not initialized. Cannot read.\n"; - } - cmd_data.copy(*m_joint_command_data_ptr); -} \ No newline at end of file diff --git a/locomotion/src/communication/src/QuadROSComm.cpp b/locomotion/src/communication/src/QuadROSComm.cpp deleted file mode 100644 index 7c4ecbd..0000000 --- a/locomotion/src/communication/src/QuadROSComm.cpp +++ /dev/null @@ -1,172 +0,0 @@ -#include "QuadROSComm.hpp" - -QuadROSComm::QuadROSComm() : m_name("svan"), rclcpp::Node("svan"), CommunicationManager(DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) { - InitClass(); -} - -QuadROSComm::QuadROSComm(const std::string& name) : m_name(name), rclcpp::Node(name), CommunicationManager(name, DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) { - InitClass(); -} - -QuadROSComm::~QuadROSComm() { - rclcpp::shutdown(); -} - -void QuadROSComm::InitClass() { - m_dt = 1./m_loop_rate; - - m_joint_state_pub = this->create_publisher("/" + m_name + "/joint_cmd", 1); - auto pub_timer = std::chrono::duration(m_dt); - m_timer = this->create_wall_timer(pub_timer, std::bind(&QuadROSComm::timer_cb, this)); - - m_joint_state_sub = this->create_subscription( - "/" + m_name + "/joint_state", - 1, - std::bind(&QuadROSComm::get_joint_state_cb, this, std::placeholders::_1) - ); - m_imu_sub = this->create_subscription( - "/" + m_name + "/imu", - 1, - std::bind(&QuadROSComm::get_imu_cb, this, std::placeholders::_1) - ); - m_odom_sub = this->create_subscription( - "/" + m_name + "/odom", - 1, - std::bind(&QuadROSComm::get_odom_cb, this, std::placeholders::_1) - ); - - js_data = sensor_msgs::msg::JointState(); - js_data.position.clear(); - js_data.velocity.clear(); - js_data.effort.clear(); - for (int i = 0; i < 12; ++i) { - js_data.position.push_back(0); - js_data.velocity.push_back(0); - js_data.effort.push_back(0); - } -} - -void QuadROSComm::get_joint_state_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const { - for (int i = 0; i < 12; ++i) { - m_sensor_data_ptr -> q(i) = msg -> position[i]; - m_sensor_data_ptr -> qd(i) = msg -> velocity[i]; - m_sensor_data_ptr -> tau(i) = msg -> effort[i]; - } -} - -void QuadROSComm::get_imu_cb(const sensor_msgs::msg::Imu::SharedPtr msg) const { - m_sensor_data_ptr -> quat(0) = msg->orientation.x; - m_sensor_data_ptr -> quat(1) = msg->orientation.y; - m_sensor_data_ptr -> quat(2) = msg->orientation.z; - m_sensor_data_ptr -> quat(3) = msg->orientation.w; - - m_sensor_data_ptr -> w_B(0) = msg->angular_velocity.x; - m_sensor_data_ptr -> w_B(1) = msg->angular_velocity.y; - m_sensor_data_ptr -> w_B(2) = msg->angular_velocity.z; - - m_sensor_data_ptr -> a_B(0) = msg->linear_acceleration.x; - m_sensor_data_ptr -> a_B(1) = msg->linear_acceleration.y; - m_sensor_data_ptr -> a_B(2) = msg->linear_acceleration.z; - - // TODO: populate a_W and w_W as well -} - -void QuadROSComm::get_odom_cb(const nav_msgs::msg::Odometry::SharedPtr msg) const { - // TODO: Use Odom data as required. Maybe extend sensor_data to have odom data? -} - -void QuadROSComm::timer_cb() { - for (int i = 0; i < 12; ++i) { - js_data.position[i] = m_joint_command_data_ptr -> q(i); - js_data.velocity[i] = m_joint_command_data_ptr -> qd(i); - js_data.effort[i] = m_joint_command_data_ptr -> tau(i); - } - m_joint_state_pub->publish(js_data); -} - -void QuadROSComm::Run() { - // rclcpp::init(0, nullptr); - rclcpp::spin(shared_from_this()); -} - -/***********************************************************/ - -CommROSP2E::CommROSP2E() : m_name("svan"), rclcpp::Node("svan"), CommunicationManager(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) { - InitClass(); -} - -CommROSP2E::CommROSP2E(const std::string& name) : m_name(name), rclcpp::Node(name), CommunicationManager(name, DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) { - InitClass(); -} - -CommROSP2E::~CommROSP2E() { - rclcpp::shutdown(); -} - -void CommROSP2E::InitClass() { - m_dt = 1./m_loop_rate; - - m_joint_state_pub = this->create_publisher("/" + m_name + "/joint_state", 1); - m_imu_pub = this->create_publisher("/" + m_name + "/imu", 1); - m_odom_pub = this->create_publisher("/" + m_name + "/odom", 1); - - auto pub_timer = std::chrono::duration(m_dt); - m_timer = this->create_wall_timer(pub_timer, std::bind(&CommROSP2E::timer_cb, this)); - - m_joint_cmd_sub = this->create_subscription( - "/" + m_name + "/joint_cmd", - 1, - std::bind(&CommROSP2E::get_joint_cmd_cb, this, std::placeholders::_1) - ); - - js_data = sensor_msgs::msg::JointState(); - imu_data = sensor_msgs::msg::Imu(); - odom_data = nav_msgs::msg::Odometry(); - - js_data.position.clear(); - js_data.velocity.clear(); - js_data.effort.clear(); - for (int i = 0; i < 12; ++i) { - js_data.position.push_back(0); - js_data.velocity.push_back(0); - js_data.effort.push_back(0); - } -} - -void CommROSP2E::get_joint_cmd_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const { - for (int i = 0; i < 12; ++i) { - m_joint_command_data_ptr -> q(i) = msg -> position[i]; - m_joint_command_data_ptr -> qd(i) = msg -> velocity[i]; - m_joint_command_data_ptr -> tau(i) = msg -> effort[i]; - } -} - -void CommROSP2E::timer_cb() { - for (int i = 0; i < 12; ++i) { - js_data.position[i] = m_sensor_data_ptr -> q(i); - js_data.velocity[i] = m_sensor_data_ptr -> qd(i); - js_data.effort[i] = m_sensor_data_ptr -> tau(i); - } - // Populate imu_data - imu_data.orientation.w = m_sensor_data_ptr -> quat(3); - imu_data.orientation.x = m_sensor_data_ptr -> quat(0); - imu_data.orientation.y = m_sensor_data_ptr -> quat(1); - imu_data.orientation.z = m_sensor_data_ptr -> quat(2); - imu_data.linear_acceleration.x = m_sensor_data_ptr -> a_B(0); - imu_data.linear_acceleration.y = m_sensor_data_ptr -> a_B(1); - imu_data.linear_acceleration.z = m_sensor_data_ptr -> a_B(2); - imu_data.angular_velocity.x = m_sensor_data_ptr -> w_B(0); - imu_data.angular_velocity.y = m_sensor_data_ptr -> w_B(1); - imu_data.angular_velocity.z = m_sensor_data_ptr -> w_B(2); - // Populdate odometry data - - // Publish stuff - m_joint_state_pub->publish(js_data); - m_imu_pub->publish(imu_data); - m_odom_pub->publish(odom_data); -} - -void CommROSP2E::Run() { - // rclcpp::init(0, nullptr); - rclcpp::spin(shared_from_this()); -} \ No newline at end of file diff --git a/locomotion/src/communication/tests/test_ros_comm.cpp b/locomotion/src/communication/tests/test_ros_comm.cpp deleted file mode 100644 index 67395ec..0000000 --- a/locomotion/src/communication/tests/test_ros_comm.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include "QuadROSComm.hpp" - -int main(int argc, char** argv) { - - rclcpp::init(argc, argv); - - std::string robot_name("go2"); - - QuadrupedCommandData cmd_data; - QuadrupedSensorData sensor_data; - auto comm_obj = std::make_shared(robot_name); - comm_obj->setSensorDataPtr(&sensor_data); - comm_obj->setCommandDataPtr(&cmd_data); - // QuadROSComm comm_obj(robot_name); - - comm_obj->Run(); - - return 0; -} \ No newline at end of file diff --git a/locomotion/src/robot_simulation/CMakeLists.txt b/locomotion/src/robot_simulation/CMakeLists.txt index eed1c87..9284168 100644 --- a/locomotion/src/robot_simulation/CMakeLists.txt +++ b/locomotion/src/robot_simulation/CMakeLists.txt @@ -1,3 +1,10 @@ +cmake_minimum_required(VERSION 3.5.1) +project(QuadrupedControl) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + + # INTERPROCEDURAL_OPTIMIZATION is enforced when enabled. set(CMAKE_POLICY_DEFAULT_CMP0069 NEW) # Default to GLVND if available. @@ -9,7 +16,7 @@ set(OpenGL_GL_PREFERENCE GLVND) # set(THREADS_PREFER_PTHREAD_FLAG ON) # find_package(mujoco CONFIG REQUIRED) -# find_package(glfw3 REQUIRED) +find_package(glfw3 REQUIRED) find_package(Threads REQUIRED) include_directories( @@ -23,32 +30,25 @@ install(DIRECTORY include/ DESTINATION ${COMMON_INCLUDE_DIR} FILES_MATCHING PATTERN "*.h*" ) - -add_executable(quad_sim src/main.cpp) -add_dependencies(quad_sim quad_communication) -target_link_libraries(quad_sim +add_executable(simulate src/sim_main.cpp) +if (DEFINED ENV{USE_ROS_COMM}) + add_definitions(-DUSE_ROS_COMM) +endif() +add_dependencies(simulate quad_communication) +target_link_libraries(simulate # mujoco::mujoco mujoco glfw Threads::Threads quad_communication + # $<$:ros_comm> ) -install(TARGETS quad_sim - ARCHIVE DESTINATION ${COMMON_INSTALL_DIR} - RUNTIME DESTINATION ${COMMON_INSTALL_DIR} -) - -add_executable(sim_ros src/main_ros.cpp) -add_dependencies(sim_ros ros_comm) -target_link_libraries(sim_ros - # mujoco::mujoco - mujoco - glfw - Threads::Threads - # quad_communication - ros_comm -) -install(TARGETS sim_ros +if (DEFINED ENV{USE_ROS_COMM}) + target_link_libraries(simulate + ros_comm + ) +endif() +install(TARGETS simulate ARCHIVE DESTINATION ${COMMON_INSTALL_DIR} RUNTIME DESTINATION ${COMMON_INSTALL_DIR} ) \ No newline at end of file diff --git a/locomotion/src/robot_simulation/include/MJSim.hpp b/locomotion/src/robot_simulation/include/MJSim.hpp deleted file mode 100644 index 5009acf..0000000 --- a/locomotion/src/robot_simulation/include/MJSim.hpp +++ /dev/null @@ -1,77 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include -#include - -// Libraries for sleep -#include -#include - -#include -#include -#include "memory_types.hpp" - -class MJSim { -public: - MJSim(); - ~MJSim(); - - virtual void CustomController(const mjModel* model, mjData* data); - - void Run(); -private: - void init_sim(); - - // keyboard callback - static void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods); - - // mouse button callback - static void mouse_button(GLFWwindow* window, int button, int act, int mods); - - // mouse move callback - static void mouse_move(GLFWwindow* window, double xpos, double ypos); - - // scroll callback - static void scroll(GLFWwindow* window, double xoffset, double yoffset); - - void UpdateSensorData(); - - std::string m_name; - - // MuJoCo data structures - mjModel* m = NULL; // MuJoCo model - mjData* d = NULL; // MuJoCo data - mjvCamera cam; // abstract camera - mjvOption opt; // visualization options - mjvScene scn; // abstract scene - mjrContext con; // custom GPU context - GLFWwindow* window; - - // mouse interaction - bool button_left = false; - bool button_middle = false; - bool button_right = false; - double lastx = 0; - double lasty = 0; - - // holders of one step history of time and position to calculate dertivatives - mjtNum position_history = 0; - mjtNum previous_time = 0; - - // controller related variables - float_t ctrl_update_freq = 100; - mjtNum last_update = 0.0; - mjtNum ctrl; - - // Variables for SHM read (Control commands: joint position, velocity, torque) - key_t read_key; - QuadrupedCommandData joint_command_data; - // Variables for SHM write (Sensor data: joint position, velocity, torque, imu data and probably base pose and contact states if required) - key_t write_key; - QuadrupedSensorData sensor_data; -}; \ No newline at end of file diff --git a/locomotion/src/communication/include/CommunicationManager.hpp b/locomotion/src/robot_simulation/include/communication/CommunicationManager.hpp similarity index 58% rename from locomotion/src/communication/include/CommunicationManager.hpp rename to locomotion/src/robot_simulation/include/communication/CommunicationManager.hpp index 60d7b4e..f10e332 100644 --- a/locomotion/src/communication/include/CommunicationManager.hpp +++ b/locomotion/src/robot_simulation/include/communication/CommunicationManager.hpp @@ -5,29 +5,40 @@ #include enum DATA_ACCESS_MODE { - PLANT_TO_EXECUTOR, - EXECUTOR_TO_PLANT + PLANT, + EXECUTOR }; // Single communication manager for quadruped class CommunicationManager { public: + CommunicationManager(); CommunicationManager(const DATA_ACCESS_MODE& mode); CommunicationManager(const std::string&, const DATA_ACCESS_MODE& mode); ~CommunicationManager() {} - void writeSensorData(const QuadrupedSensorData& sensor_data); - void writeCommandData(const QuadrupedCommandData& cmd_data); - void setSensorDataPtr(QuadrupedSensorData* sensor_data_ptr) { m_sensor_data_ptr = sensor_data_ptr; } void setCommandDataPtr(QuadrupedCommandData* command_data_ptr) { m_joint_command_data_ptr = command_data_ptr; } + void setMeasurementDataPtr(QuadrupedMeasurementData* measurement_data_ptr) { + m_measurement_data_ptr = measurement_data_ptr; + } + void setEstimationDataPtr(QuadrupedEstimationData* estimation_data_ptr) { + m_estimation_data_ptr = estimation_data_ptr; + } + void writeSensorData(const QuadrupedSensorData& sensor_data); + void writeCommandData(const QuadrupedCommandData& cmd_data); + void writeMeasurementData(const QuadrupedMeasurementData& measure_data); + void writeEstimationData(const QuadrupedEstimationData& est_data); + void getSensorData(QuadrupedSensorData& sensor_data); void getCommandData(QuadrupedCommandData& cmd_data); + void getMeasurememtData(QuadrupedMeasurementData& measure_data); + void getEstimationData(QuadrupedEstimationData& est_data); void setAccessMode(const DATA_ACCESS_MODE& mode) { m_mode = mode; @@ -36,10 +47,10 @@ public: protected: QuadrupedSensorData* m_sensor_data_ptr = NULL; QuadrupedCommandData* m_joint_command_data_ptr = NULL; + QuadrupedEstimationData* m_estimation_data_ptr = NULL; + QuadrupedMeasurementData* m_measurement_data_ptr = NULL; - int m_mode = DATA_ACCESS_MODE::PLANT_TO_EXECUTOR; + int m_mode = DATA_ACCESS_MODE::PLANT; private: std::string m_name; - // QuadrupedEstimatoinData m_estimation_data; - // QuadrupedPlannerData m_planner_data; }; \ No newline at end of file diff --git a/locomotion/src/communication/include/QuadROSComm.hpp b/locomotion/src/robot_simulation/include/communication/QuadROSComm.hpp similarity index 51% rename from locomotion/src/communication/include/QuadROSComm.hpp rename to locomotion/src/robot_simulation/include/communication/QuadROSComm.hpp index ec58759..70f842f 100644 --- a/locomotion/src/communication/include/QuadROSComm.hpp +++ b/locomotion/src/robot_simulation/include/communication/QuadROSComm.hpp @@ -10,19 +10,28 @@ #include "sensor_msgs/msg/joint_state.hpp" #include "sensor_msgs/msg/imu.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "std_msgs/msg/float32.hpp" using namespace std::chrono_literals; // using std::placeholders::_1; class QuadROSComm : public rclcpp::Node, public CommunicationManager { public: - QuadROSComm(); - QuadROSComm(const std::string& name); + QuadROSComm(const DATA_ACCESS_MODE& mode); + QuadROSComm(const std::string& name, const DATA_ACCESS_MODE& mode); ~QuadROSComm(); + void setUpdateRate(const float& rate) { + m_loop_rate = rate; + m_dt = 1./rate; + } + void Run(); -private: + void InitClass(); +private: std::string m_name; float m_loop_rate = 1000; float m_dt = 0.001; @@ -32,6 +41,23 @@ private: rclcpp::Subscription::SharedPtr m_odom_sub; // Publisher(s) rclcpp::Publisher::SharedPtr m_joint_state_pub; + rclcpp::Publisher::SharedPtr m_est_cs_pub[4]; + rclcpp::Publisher::SharedPtr m_est_pc_pub[4]; + rclcpp::Publisher::SharedPtr m_est_base_position_pub[3]; + rclcpp::Publisher::SharedPtr m_est_base_velocity_pub[3]; + + // Subscriber(s) + rclcpp::Subscription::SharedPtr m_joint_cmd_sub; + // Publisher(s) + // rclcpp::Publisher::SharedPtr m_joint_state_pub; + rclcpp::Publisher::SharedPtr m_imu_pub; + rclcpp::Publisher::SharedPtr m_odom_pub; + // Measurement publishers + rclcpp::Publisher::SharedPtr m_base_position_pub; + rclcpp::Publisher::SharedPtr m_base_vel_pub; + rclcpp::Publisher::SharedPtr m_base_acc_pub; + rclcpp::Publisher::SharedPtr m_contact_forces_pub[12]; + rclcpp::Publisher::SharedPtr m_estimated_fc_pub[12]; // Timer rclcpp::TimerBase::SharedPtr m_timer; @@ -40,40 +66,23 @@ private: void get_joint_state_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const; void get_imu_cb(const sensor_msgs::msg::Imu::SharedPtr msg) const; void get_odom_cb(const nav_msgs::msg::Odometry::SharedPtr msg) const; - // Timer callback - void timer_cb(); - - sensor_msgs::msg::JointState js_data; -}; - -class CommROSP2E : public rclcpp::Node, public CommunicationManager { -public: - CommROSP2E(); - CommROSP2E(const std::string& name); - ~CommROSP2E(); - - void Run(); -private: - void InitClass(); - std::string m_name; - float m_loop_rate = 1000; - float m_dt = 0.001; - // Subscriber(s) - rclcpp::Subscription::SharedPtr m_joint_cmd_sub; - // Publisher(s) - rclcpp::Publisher::SharedPtr m_joint_state_pub; - rclcpp::Publisher::SharedPtr m_imu_pub; - rclcpp::Publisher::SharedPtr m_odom_pub; - - // Timer - rclcpp::TimerBase::SharedPtr m_timer; - // Subscription callbacks void get_joint_cmd_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const; // Timer callback void timer_cb(); + // sensor_msgs::msg::JointState js_data; + std_msgs::msg::Float32 est_cs_data[4]; + std_msgs::msg::Float32 est_pc_data[4]; + std_msgs::msg::Float32 est_base_pos_data[3]; + std_msgs::msg::Float32 est_base_vel_data[3]; + sensor_msgs::msg::JointState js_data; sensor_msgs::msg::Imu imu_data; nav_msgs::msg::Odometry odom_data; + geometry_msgs::msg::Point base_position_data; + geometry_msgs::msg::Twist base_velocity_data; + geometry_msgs::msg::Twist base_acceleration_data; + std_msgs::msg::Float32 contact_forces_data[12]; + std_msgs::msg::Float32 estimated_fc_data[12]; }; \ No newline at end of file diff --git a/locomotion/src/communication/include/SHM.hpp b/locomotion/src/robot_simulation/include/communication/SHM.hpp similarity index 95% rename from locomotion/src/communication/include/SHM.hpp rename to locomotion/src/robot_simulation/include/communication/SHM.hpp index f51867b..756b0cd 100644 --- a/locomotion/src/communication/include/SHM.hpp +++ b/locomotion/src/robot_simulation/include/communication/SHM.hpp @@ -7,6 +7,7 @@ // Randomy chosen key enums enum SHM_KEYS { SENSOR_DATA = 123, + MEASUREMENT_DATA = 496, COMMAND_DATA = 549 }; diff --git a/locomotion/src/robot_simulation/include/mjsim_helper.hpp b/locomotion/src/robot_simulation/include/mjsim_helper.hpp deleted file mode 100644 index 65a05e4..0000000 --- a/locomotion/src/robot_simulation/include/mjsim_helper.hpp +++ /dev/null @@ -1,248 +0,0 @@ -#include -#include -#include -#include - -#include -#include - -// Libraries for sleep -#include -#include - -#include "memory_types.hpp" -#include "SHM.hpp" - -#define COMPILE_WITH_VISUALIZATION - -// MuJoCo data structures -mjModel* m = NULL; // MuJoCo model -mjData* d = NULL; // MuJoCo data - -#ifdef COMPILE_WITH_VISUALIZATION - mjvCamera cam; // abstract camera - mjvOption opt; // visualization options - mjvScene scn; // abstract scene - mjrContext con; // custom GPU context - GLFWwindow* window; - - // mouse interaction - bool button_left = false; - bool button_middle = false; - bool button_right = false; - double lastx = 0; - double lasty = 0; -#endif - -// holders of one step history of time and position to calculate dertivatives -mjtNum position_history = 0; -mjtNum previous_time = 0; -mjtNum timezero = 0; - -// controller related variables -float_t ctrl_update_freq = 500; -mjtNum last_update = 0.0; -mjtNum ctrl; - -// // Variables for SHM read (Control commands: joint position, velocity, torque) -// key_t read_key; -// QuadrupedCommandData joint_command_data; -// // Variables for SHM write (Sensor data: joint position, velocity, torque, imu data and probably base pose and contact states if required) -// key_t write_key; -// QuadrupedSensorData sensor_data; - -QuadrupedSensorData sensor_data; -QuadrupedCommandData joint_command_data; -SHM comm_data(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR); - - -#ifdef COMPILE_WITH_VISUALIZATION -// keyboard callback -void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods) { - // backspace: reset simulation - if (act==GLFW_PRESS && key==GLFW_KEY_BACKSPACE) { - mj_resetData(m, d); - mj_forward(m, d); - } -} - -// mouse button callback -void mouse_button(GLFWwindow* window, int button, int act, int mods) { - // update button state - button_left = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT)==GLFW_PRESS); - button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE)==GLFW_PRESS); - button_right = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT)==GLFW_PRESS); - - // update mouse position - glfwGetCursorPos(window, &lastx, &lasty); -} - -// mouse move callback -void mouse_move(GLFWwindow* window, double xpos, double ypos) { - // no buttons down: nothing to do - if (!button_left && !button_middle && !button_right) { - return; - } - - // compute mouse displacement, save - double dx = xpos - lastx; - double dy = ypos - lasty; - lastx = xpos; - lasty = ypos; - - // get current window size - int width, height; - glfwGetWindowSize(window, &width, &height); - - // get shift key state - bool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS || - glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS); - - // determine action based on mouse button - mjtMouse action; - if (button_right) { - action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V; - } else if (button_left) { - action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V; - } else { - action = mjMOUSE_ZOOM; - } - - // move camera - mjv_moveCamera(m, action, dx/height, dy/height, &scn, &cam); -} - -// scroll callback -void scroll(GLFWwindow* window, double xoffset, double yoffset) { - // emulate vertical mouse motion = 5% of window height - mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05*yoffset, &scn, &cam); -} -#endif - -void UpdateSensorData(); -void CustomController(const mjModel*, mjData*); - -void init_model(std::string model_file) { - // load and compile model - char error[1000] = "Could not load binary model"; - if (std::strlen(model_file.c_str())>4 && !std::strcmp(model_file.c_str() + std::strlen(model_file.c_str())-4, ".mjb")) { - m = mj_loadModel(model_file.c_str(), 0); - } else { - m = mj_loadXML(model_file.c_str(), 0, error, 1000); - } - if (!m) { - mju_error("Load model error: %s", error); - } - // make data - d = mj_makeData(m); -} - -#ifdef COMPILE_WITH_VISUALIZATION -void init_visualizer() { - // init GLFW - if (!glfwInit()) { - mju_error("Could not initialize GLFW"); - } - - // create window, make OpenGL context current, request v-sync - window = glfwCreateWindow(1200, 900, "Demo", NULL, NULL); - glfwMakeContextCurrent(window); - glfwSwapInterval(1); - - // initialize visualization data structures - mjv_defaultCamera(&cam); - mjv_defaultOption(&opt); - mjv_defaultScene(&scn); - mjr_defaultContext(&con); - - // create scene and context - mjv_makeScene(m, &scn, 2000); - mjr_makeContext(m, &con, mjFONTSCALE_150); - - // install GLFW mouse and keyboard callbacks - glfwSetKeyCallback(window, keyboard); - glfwSetCursorPosCallback(window, mouse_move); - glfwSetMouseButtonCallback(window, mouse_button); - glfwSetScrollCallback(window, scroll); -} -#endif - -void run_simulation(float sim_time = 1, bool show_visualization=false, float fps=60.) { -#ifndef COMPILE_WITH_VISUALIZATION - if (show_visualization) { - std::cerr << "Exec compiled with Visuzlization disabled. Re-compile with COMPILE_WITH_VISUALIZATION flag enabled to start visualization!\n"; - } -#endif -#ifdef COMPILE_WITH_VISUALIZATION - if (show_visualization) { - init_visualizer(); - // use the first while condition if you want to simulate for a period. - while( !glfwWindowShouldClose(window) && d->time-timezero < sim_time) { - // while( !glfwWindowShouldClose(window)) { - // advance interactive simulation for 1/60 sec - // Assuming MuJoCo can simulate faster than real-time, which it usually can, - // this loop will finish on time for the next frame to be rendered at 60 fps. - // Otherwise add a cpu timer and exit this loop when it is time to render. -#endif - mjtNum simstart = d->time; -#ifdef COMPILE_WITH_VISUALIZATION - while( d->time - simstart < 1.0/fps ) { -#else - while (d->time - simstart < sim_time) { -#endif - UpdateSensorData(); - mj_step(m, d); - } -#ifdef COMPILE_WITH_VISUALIZATION - // 15 ms is a little smaller than 60 Hz. - int delay_ms = int(double(1e3)/fps); - std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); - // get framebuffer viewport - mjrRect viewport = {0, 0, 0, 0}; - glfwGetFramebufferSize(window, &viewport.width, &viewport.height); - - // update scene and render - mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn); - mjr_render(viewport, &scn, &con); - - // swap OpenGL buffers (blocking call due to v-sync) - glfwSwapBuffers(window); - - // process pending GUI events, call GLFW callbacks - glfwPollEvents(); - - } - } else { - mjtNum simstart = d->time; - float dt = 0; - if (fps == 0) { - dt = sim_time; - } else { - dt = 1./fps; - } - while (d->time - simstart < dt) { - UpdateSensorData(); - mj_step(m, d); - } - } -#endif -} - -void shutdown_simulation() { - // free visualization storage -#ifdef COMPILE_WITH_VISUALIZATION - mjv_freeScene(&scn); - mjr_freeContext(&con); -#endif - - // free MuJoCo model and data, deactivate - mj_deleteData(d); - mj_deleteModel(m); - -#ifdef COMPILE_WITH_VISUALIZATION - // terminate GLFW (crashes with Linux NVidia drivers) - #if defined(__APPLE__) || defined(_WIN32) - glfwTerminate(); - #endif -#endif -} \ No newline at end of file diff --git a/locomotion/src/robot_simulation/include/quad_simulator.hpp b/locomotion/src/robot_simulation/include/quad_simulator.hpp deleted file mode 100644 index e6aa9b2..0000000 --- a/locomotion/src/robot_simulation/include/quad_simulator.hpp +++ /dev/null @@ -1,325 +0,0 @@ -// Copyright 2021 DeepMind Technologies Limited -// -// 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. - -#ifndef MUJOCO_SIMULATE_SIMULATE_H_ -#define MUJOCO_SIMULATE_SIMULATE_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include "platform_ui_adapter.h" - -namespace mujoco { - -// The viewer itself doesn't require a reentrant mutex, however we use it in -// order to provide a Python sync API that doesn't require separate locking -// (since sync is by far the most common operation), but that also won't -// deadlock if called when a lock is already held by the user script on the -// same thread. -class SimulateMutex : public std::recursive_mutex {}; -using MutexLock = std::unique_lock; - -// Simulate states not contained in MuJoCo structures -class Simulate { - public: - using Clock = std::chrono::steady_clock; - static_assert(std::ratio_less_equal_v); - - static constexpr int kMaxGeom = 20000; - - // create object and initialize the simulate ui - Simulate( - std::unique_ptr platform_ui_adapter, - mjvCamera* cam, mjvOption* opt, mjvPerturb* pert, bool is_passive); - - // Synchronize mjModel and mjData state with UI inputs, and update - // visualization. - void Sync(); - - void UpdateHField(int hfieldid); - void UpdateMesh(int meshid); - void UpdateTexture(int texid); - - // Request that the Simulate UI display a "loading" message - // Called prior to Load or LoadMessageClear - void LoadMessage(const char* displayed_filename); - - // Request that the Simulate UI thread render a new model - void Load(mjModel* m, mjData* d, const char* displayed_filename); - - // Clear the loading message - // Can be called instead of Load to clear the message without - // requesting the UI load a model - void LoadMessageClear(void); - - // functions below are used by the renderthread - // load mjb or xml model that has been requested by load() - void LoadOnRenderThread(); - - // render the ui to the window - void Render(); - - // loop to render the UI (must be called from main thread because of MacOS) - void RenderLoop(); - - // add state to history buffer - void AddToHistory(); - - // constants - static constexpr int kMaxFilenameLength = 1000; - - // whether the viewer is operating in passive mode, where it cannot assume - // that it has exclusive access to mjModel, mjData, and various mjv objects - bool is_passive_ = false; - - // model and data to be visualized - mjModel* mnew_ = nullptr; - mjData* dnew_ = nullptr; - - mjModel* m_ = nullptr; - mjData* d_ = nullptr; - - int ncam_ = 0; - int nkey_ = 0; - int state_size_ = 0; // number of mjtNums in a history buffer state - int nhistory_ = 0; // number of states saved in history buffer - int history_cursor_ = 0; // cursor pointing at last saved state - - std::vector body_parentid_; - - std::vector jnt_type_; - std::vector jnt_group_; - std::vector jnt_qposadr_; - std::vector>> jnt_range_; - std::vector jnt_names_; - - std::vector actuator_group_; - std::vector>> actuator_ctrlrange_; - std::vector actuator_names_; - - std::vector history_; // history buffer (nhistory x state_size) - - // mjModel and mjData fields that can be modified by the user through the GUI - std::vector qpos_; - std::vector qpos_prev_; - std::vector ctrl_; - std::vector ctrl_prev_; - - mjvSceneState scnstate_; - mjOption mjopt_prev_; - mjvOption opt_prev_; - mjvCamera cam_prev_; - - int warn_vgeomfull_prev_; - - // pending GUI-driven actions, to be applied at the next call to Sync - struct { - std::optional save_xml; - std::optional save_mjb; - std::optional print_model; - std::optional print_data; - bool reset; - bool align; - bool copy_pose; - bool load_from_history; - bool load_key; - bool save_key; - bool zero_ctrl; - int newperturb; - bool select; - mjuiState select_state; - bool ui_update_simulation; - bool ui_update_physics; - bool ui_update_rendering; - bool ui_update_joint; - bool ui_update_ctrl; - bool ui_remake_ctrl; - } pending_ = {}; - - SimulateMutex mtx; - std::condition_variable_any cond_loadrequest; - - int frames_ = 0; - std::chrono::time_point last_fps_update_; - double fps_ = 0; - - // options - int spacing = 0; - int color = 0; - int font = 0; - int ui0_enable = 1; - int ui1_enable = 1; - int help = 0; - int info = 0; - int profiler = 0; - int sensor = 0; - int pause_update = 1; - int fullscreen = 0; - int vsync = 1; - int busywait = 0; - - // keyframe index - int key = 0; - - // index of history-scrubber slider - int scrub_index = 0; - - // simulation - int run = 1; - - // atomics for cross-thread messages - std::atomic_int exitrequest = 0; - std::atomic_int droploadrequest = 0; - std::atomic_int screenshotrequest = 0; - std::atomic_int uiloadrequest = 0; - - // loadrequest - // 3: display a loading message - // 2: render thread asked to update its model - // 1: showing "loading" label, about to load - // 0: model loaded or no load requested. - int loadrequest = 0; - - // strings - char load_error[kMaxFilenameLength] = ""; - char dropfilename[kMaxFilenameLength] = ""; - char filename[kMaxFilenameLength] = ""; - char previous_filename[kMaxFilenameLength] = ""; - - // time synchronization - int real_time_index = 0; - bool speed_changed = true; - float measured_slowdown = 1.0; - // logarithmically spaced real-time slow-down coefficients (percent) - static constexpr float percentRealTime[] = { - 100, 80, 66, 50, 40, 33, 25, 20, 16, 13, - 10, 8, 6.6, 5.0, 4, 3.3, 2.5, 2, 1.6, 1.3, - 1, .8, .66, .5, .4, .33, .25, .2, .16, .13, - .1 - }; - - // control noise - double ctrl_noise_std = 0.0; - double ctrl_noise_rate = 0.0; - - // watch - char field[mjMAXUITEXT] = "qpos"; - int index = 0; - - // physics: need sync - int disable[mjNDISABLE] = {0}; - int enable[mjNENABLE] = {0}; - int enableactuator[mjNGROUP] = {0}; - - // rendering: need sync - int camera = 0; - - // abstract visualization - mjvScene scn; - mjvCamera& cam; - mjvOption& opt; - mjvPerturb& pert; - mjvFigure figconstraint = {}; - mjvFigure figcost = {}; - mjvFigure figtimer = {}; - mjvFigure figsize = {}; - mjvFigure figsensor = {}; - - // additional user-defined visualization geoms (used in passive mode) - mjvScene* user_scn = nullptr; - mjtByte user_scn_flags_prev_[mjNRNDFLAG]; - - // OpenGL rendering and UI - int refresh_rate = 60; - int window_pos[2] = {0}; - int window_size[2] = {0}; - std::unique_ptr platform_ui; - mjuiState& uistate; - mjUI ui0 = {}; - mjUI ui1 = {}; - - // Constant arrays needed for the option section of UI and the UI interface - // TODO setting the size here is not ideal - const mjuiDef def_option[13] = { - {mjITEM_SECTION, "Option", 1, nullptr, "AO"}, - {mjITEM_CHECKINT, "Help", 2, &this->help, " #290"}, - {mjITEM_CHECKINT, "Info", 2, &this->info, " #291"}, - {mjITEM_CHECKINT, "Profiler", 2, &this->profiler, " #292"}, - {mjITEM_CHECKINT, "Sensor", 2, &this->sensor, " #293"}, - {mjITEM_CHECKINT, "Pause update", 2, &this->pause_update, ""}, - #ifdef __APPLE__ - {mjITEM_CHECKINT, "Fullscreen", 0, &this->fullscreen, " #294"}, - #else - {mjITEM_CHECKINT, "Fullscreen", 1, &this->fullscreen, " #294"}, - #endif - {mjITEM_CHECKINT, "Vertical Sync", 1, &this->vsync, ""}, - {mjITEM_CHECKINT, "Busy Wait", 1, &this->busywait, ""}, - {mjITEM_SELECT, "Spacing", 1, &this->spacing, "Tight\nWide"}, - {mjITEM_SELECT, "Color", 1, &this->color, "Default\nOrange\nWhite\nBlack"}, - {mjITEM_SELECT, "Font", 1, &this->font, "50 %\n100 %\n150 %\n200 %\n250 %\n300 %"}, - {mjITEM_END} - }; - - - // simulation section of UI - const mjuiDef def_simulation[14] = { - {mjITEM_SECTION, "Simulation", 1, nullptr, "AS"}, - {mjITEM_RADIO, "", 5, &this->run, "Pause\nRun"}, - {mjITEM_BUTTON, "Reset", 2, nullptr, " #259"}, - {mjITEM_BUTTON, "Reload", 5, nullptr, "CL"}, - {mjITEM_BUTTON, "Align", 2, nullptr, "CA"}, - {mjITEM_BUTTON, "Copy pose", 2, nullptr, "CC"}, - {mjITEM_SLIDERINT, "Key", 3, &this->key, "0 0"}, - {mjITEM_BUTTON, "Load key", 3}, - {mjITEM_BUTTON, "Save key", 3}, - {mjITEM_SLIDERNUM, "Noise scale", 5, &this->ctrl_noise_std, "0 2"}, - {mjITEM_SLIDERNUM, "Noise rate", 5, &this->ctrl_noise_rate, "0 2"}, - {mjITEM_SEPARATOR, "History", 1}, - {mjITEM_SLIDERINT, "", 5, &this->scrub_index, "0 0"}, - {mjITEM_END} - }; - - - // watch section of UI - const mjuiDef def_watch[5] = { - {mjITEM_SECTION, "Watch", 0, nullptr, "AW"}, - {mjITEM_EDITTXT, "Field", 2, this->field, "qpos"}, - {mjITEM_EDITINT, "Index", 2, &this->index, "1"}, - {mjITEM_STATIC, "Value", 2, nullptr, " "}, - {mjITEM_END} - }; - - // info strings - char info_title[Simulate::kMaxFilenameLength] = {0}; - char info_content[Simulate::kMaxFilenameLength] = {0}; - - // pending uploads - std::condition_variable_any cond_upload_; - int texture_upload_ = -1; - int mesh_upload_ = -1; - int hfield_upload_ = -1; -}; -} // namespace mujoco - -#endif diff --git a/locomotion/src/robot_simulation/include/mjsim_helper_ros.hpp b/locomotion/src/robot_simulation/include/sim_helper.hpp similarity index 97% rename from locomotion/src/robot_simulation/include/mjsim_helper_ros.hpp rename to locomotion/src/robot_simulation/include/sim_helper.hpp index 8d3aaf9..523dfc7 100644 --- a/locomotion/src/robot_simulation/include/mjsim_helper_ros.hpp +++ b/locomotion/src/robot_simulation/include/sim_helper.hpp @@ -11,7 +11,11 @@ #include #include "memory_types.hpp" +#ifdef USE_ROS_COMM #include "QuadROSComm.hpp" +#else +#include "SHM.hpp" +#endif #define COMPILE_WITH_VISUALIZATION @@ -53,8 +57,13 @@ mjtNum ctrl; QuadrupedSensorData sensor_data; QuadrupedCommandData joint_command_data; +QuadrupedMeasurementData measurement_data; // SHM comm_data(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR); -std::shared_ptr comm_data_ptr; +#ifdef USE_ROS_COMM +std::shared_ptr comm_data_ptr; +#else +std::shared_ptr comm_data_ptr; +#endif #ifdef COMPILE_WITH_VISUALIZATION diff --git a/locomotion/src/utils/include/memory_types.hpp b/locomotion/src/robot_simulation/include/utils/memory_types.hpp similarity index 67% rename from locomotion/src/utils/include/memory_types.hpp rename to locomotion/src/robot_simulation/include/utils/memory_types.hpp index 8b417b7..01affec 100644 --- a/locomotion/src/utils/include/memory_types.hpp +++ b/locomotion/src/robot_simulation/include/utils/memory_types.hpp @@ -61,7 +61,6 @@ typedef struct QuadrupedEstimationData { vec12 rP; int4 cs; vec4 pc; - int num_contact; vec19 js; vec18 jv; QuadrupedEstimationData() { @@ -80,6 +79,8 @@ typedef struct QuadrupedEstimationData { this->rP = est.rP; this->cs = est.cs; this->pc = est.pc; + this->js = est.js; + this->jv = est.jv; } } QuadrupedEstimationData; @@ -113,4 +114,37 @@ typedef struct QuadrupedPlannerData { cs_ref = int4::Zero(); pc_ref = vec4::Zero(); } -} QuadrupedPlannerData; \ No newline at end of file +} QuadrupedPlannerData; + +typedef struct QuadrupedMeasurementData { + vec3 base_position; + // A better name might be PluckerVector? + struct TotalVelocity { + vec3 linear; + vec3 angular; + TotalVelocity() { + linear = vec3::Zero(); + angular = vec3::Zero(); + } + }; + TotalVelocity base_velocity; + TotalVelocity base_acceleration; + vec12 contact_force; + vec12 estimated_contact_force; + QuadrupedMeasurementData() { + base_position = vec3::Zero(); + base_velocity = TotalVelocity(); + base_acceleration = TotalVelocity(); + contact_force = vec12::Zero(); + estimated_contact_force = vec12::Zero(); + } + void copy(const QuadrupedMeasurementData& m) { + this->base_position = m.base_position; + this->base_velocity.linear = m.base_velocity.linear; + this->base_velocity.angular = m.base_velocity.angular; + this->base_acceleration.linear = m.base_acceleration.linear; + this->base_acceleration.angular = m.base_acceleration.angular; + this->contact_force = m.contact_force; + this->estimated_contact_force = m.estimated_contact_force; + } +} QuadrupedMeasurementData; \ No newline at end of file diff --git a/locomotion/src/utils/include/timer.hpp b/locomotion/src/robot_simulation/include/utils/timer.hpp similarity index 100% rename from locomotion/src/utils/include/timer.hpp rename to locomotion/src/robot_simulation/include/utils/timer.hpp diff --git a/locomotion/src/robot_simulation/include/utils/utils.hpp b/locomotion/src/robot_simulation/include/utils/utils.hpp new file mode 100644 index 0000000..8e85c86 --- /dev/null +++ b/locomotion/src/robot_simulation/include/utils/utils.hpp @@ -0,0 +1,150 @@ +#pragma once + +#include +#include + +template int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +namespace pinocchio { + inline mat3x3 skew_symm(const vec3 &v) { + mat3x3 vx; + vx << 0, -v(2), v(1), + v(2), 0, -v(0), + -v(1), v(0), 0; + + return vx; + } + + inline mat3x3 Rx(float x) + { + mat3x3 R = mat3x3::Zero(); + + R << 1, 0, 0, + 0, cos(x), -sin(x), + 0, sin(x), cos(x); + + return R; + } + inline mat3x3 Ry(float x) + { + mat3x3 R = mat3x3::Zero(); + + R << cos(x), 0, sin(x), + 0, 1, 0, + -sin(x), 0, cos(x); + + return R; + } + inline mat3x3 Rz(float x) + { + mat3x3 R = mat3x3::Zero(); + + R << cos(x), -sin(x), 0, + sin(x), cos(x), 0, + 0, 0, 1; + + return R; + } + + // Rewrite directly the analytical expression + inline mat3x3 EulXYZ2Rot(const vec3 &eul) + { + // XYZ intrinsic euler angles, converts from base to global + return Rx(eul(0)) * Ry(eul(1)) * Rz(eul(2)); + } + + inline mat3x3 quat2rot(const vec4 &q) { + // pinocchio convention of {x, y, z, w} + float q0 = q(3); + vec3 qv = q.block<3, 1>(0, 0); + mat3x3 Rot = (2 * q0 * q0 - 1) * mat3x3::Identity() + 2 * q0 * skew_symm(qv) + 2 * qv * qv.transpose(); + + return Rot; + } + + inline vec4 zeta(const vec3 &v) { + float v_norm = v.norm(); + vec4 q = vec4(0, 0, 0, 1); + q(3) = cos(v_norm / 2); + if (v_norm == 0) { + q.block<3, 1>(0, 0) = vec3::Zero(3); + } else { + q.block<3, 1>(0, 0) = v * sin(v_norm / 2) / v_norm; + } + return q; + } + + inline vec4 Rot2AxisAngle(const mat3x3 &R) { + vec4 axang = vec4::Zero(); + + double tmp = (R.trace() - 1) / 2; + axang.block<3, 1>(1, 0) = vec3(0, 0, 0); + if (tmp >= 1.) { + axang(0) = 0; + } else if (tmp <= -1.) { + axang(0) = M_PI; + } else { + axang(0) = std::acos(tmp); + axang.block<3, 1>(1, 0) = (0.5 / sin(axang(0))) * vec3(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); + } + + return axang; + } + + inline vec3 Rot2Phi(const mat3x3 &R) { + vec4 axang = Rot2AxisAngle(R); + + return axang(0) * axang.block<3, 1>(1, 0); + } + + inline vec4 EulXYZ2quat(const vec3& eul) { + return zeta(Rot2Phi(EulXYZ2Rot(eul))); + } + + inline vec3 Rot2EulXYZ(const mat3x3& R) { + // reference: https://www.geometrictools.com/Documentation/EulerAngles.pdf : Page 4-5 + vec3 eul(0, 0, 0); + if (R(0, 2) < 1) { + if (R(0, 2) > -1) { + eul(0) = atan2(-R(1, 2), R(2, 2)); + eul(1) = asin(R(0, 2)); + eul(2) = atan2(-R(0, 1), R(0, 0)); + } else { + eul(0) = -atan2(R(1, 0), R(1, 1)); + eul(1) = -M_PI / 2; + eul(2) = 0; + } + } else { + eul(0) = atan2(R(1, 0), R(1, 1)); + eul(1) = M_PI / 2; + eul(2) = 0; + } + + return eul; + } + inline vec3 matrixLogRot(const mat3x3 &R) { + double theta; + + double tmp = (R.trace() - 1) / 2; + if (tmp >= 1.) { + theta = 0; + } else if (tmp <= -1.) { + theta = M_PI; + } else { + theta = std::acos(tmp); + } + + vec3 omega = vec3(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); + + if (theta > 1e-4) { + omega *= theta / (2 * sin(theta)); + } + else { + omega /= 2; + } + + return omega; + } +} \ No newline at end of file diff --git a/locomotion/src/robots/.gitignore b/locomotion/src/robot_simulation/robots/.gitignore similarity index 100% rename from locomotion/src/robots/.gitignore rename to locomotion/src/robot_simulation/robots/.gitignore diff --git a/locomotion/src/robots/LICENSE b/locomotion/src/robot_simulation/robots/LICENSE similarity index 100% rename from locomotion/src/robots/LICENSE rename to locomotion/src/robot_simulation/robots/LICENSE diff --git a/locomotion/src/robots/b1_dae_to_stl_decimation.blend b/locomotion/src/robot_simulation/robots/b1_dae_to_stl_decimation.blend similarity index 100% rename from locomotion/src/robots/b1_dae_to_stl_decimation.blend rename to locomotion/src/robot_simulation/robots/b1_dae_to_stl_decimation.blend diff --git a/locomotion/src/robots/b1_dae_to_stl_decimation.blend1 b/locomotion/src/robot_simulation/robots/b1_dae_to_stl_decimation.blend1 similarity index 100% rename from locomotion/src/robots/b1_dae_to_stl_decimation.blend1 rename to locomotion/src/robot_simulation/robots/b1_dae_to_stl_decimation.blend1 diff --git a/locomotion/src/robots/b1_description/CMakeLists.txt b/locomotion/src/robot_simulation/robots/b1_description/CMakeLists.txt similarity index 100% rename from locomotion/src/robots/b1_description/CMakeLists.txt rename to locomotion/src/robot_simulation/robots/b1_description/CMakeLists.txt diff --git a/locomotion/src/robots/b1_description/config/robot_control.yaml b/locomotion/src/robot_simulation/robots/b1_description/config/robot_control.yaml similarity index 100% rename from locomotion/src/robots/b1_description/config/robot_control.yaml rename to locomotion/src/robot_simulation/robots/b1_description/config/robot_control.yaml diff --git a/locomotion/src/robots/b1_description/launch/b1_rviz.launch b/locomotion/src/robot_simulation/robots/b1_description/launch/b1_rviz.launch similarity index 100% rename from locomotion/src/robots/b1_description/launch/b1_rviz.launch rename to locomotion/src/robot_simulation/robots/b1_description/launch/b1_rviz.launch diff --git a/locomotion/src/robots/b1_description/launch/check_joint.rviz b/locomotion/src/robot_simulation/robots/b1_description/launch/check_joint.rviz similarity index 100% rename from locomotion/src/robots/b1_description/launch/check_joint.rviz rename to locomotion/src/robot_simulation/robots/b1_description/launch/check_joint.rviz diff --git a/locomotion/src/robots/b1_description/meshes/calf.dae b/locomotion/src/robot_simulation/robots/b1_description/meshes/calf.dae similarity index 100% rename from locomotion/src/robots/b1_description/meshes/calf.dae rename to locomotion/src/robot_simulation/robots/b1_description/meshes/calf.dae diff --git a/locomotion/src/robots/b1_description/meshes/calf.stl b/locomotion/src/robot_simulation/robots/b1_description/meshes/calf.stl similarity index 100% rename from locomotion/src/robots/b1_description/meshes/calf.stl rename to locomotion/src/robot_simulation/robots/b1_description/meshes/calf.stl diff --git a/locomotion/src/robots/b1_description/meshes/hip.dae b/locomotion/src/robot_simulation/robots/b1_description/meshes/hip.dae similarity index 100% rename from locomotion/src/robots/b1_description/meshes/hip.dae rename to locomotion/src/robot_simulation/robots/b1_description/meshes/hip.dae diff --git a/locomotion/src/robots/b1_description/meshes/hip.stl b/locomotion/src/robot_simulation/robots/b1_description/meshes/hip.stl similarity index 100% rename from locomotion/src/robots/b1_description/meshes/hip.stl rename to locomotion/src/robot_simulation/robots/b1_description/meshes/hip.stl diff --git a/locomotion/src/robots/b1_description/meshes/thigh.dae b/locomotion/src/robot_simulation/robots/b1_description/meshes/thigh.dae similarity index 100% rename from locomotion/src/robots/b1_description/meshes/thigh.dae rename to locomotion/src/robot_simulation/robots/b1_description/meshes/thigh.dae diff --git a/locomotion/src/robots/b1_description/meshes/thigh.stl b/locomotion/src/robot_simulation/robots/b1_description/meshes/thigh.stl similarity index 100% rename from locomotion/src/robots/b1_description/meshes/thigh.stl rename to locomotion/src/robot_simulation/robots/b1_description/meshes/thigh.stl diff --git a/locomotion/src/robots/b1_description/meshes/thigh_mirror.dae b/locomotion/src/robot_simulation/robots/b1_description/meshes/thigh_mirror.dae similarity index 100% rename from locomotion/src/robots/b1_description/meshes/thigh_mirror.dae rename to locomotion/src/robot_simulation/robots/b1_description/meshes/thigh_mirror.dae diff --git a/locomotion/src/robots/b1_description/meshes/thigh_mirror.stl b/locomotion/src/robot_simulation/robots/b1_description/meshes/thigh_mirror.stl similarity index 100% rename from locomotion/src/robots/b1_description/meshes/thigh_mirror.stl rename to locomotion/src/robot_simulation/robots/b1_description/meshes/thigh_mirror.stl diff --git a/locomotion/src/robots/b1_description/meshes/trunk.dae b/locomotion/src/robot_simulation/robots/b1_description/meshes/trunk.dae similarity index 100% rename from locomotion/src/robots/b1_description/meshes/trunk.dae rename to locomotion/src/robot_simulation/robots/b1_description/meshes/trunk.dae diff --git a/locomotion/src/robots/b1_description/meshes/trunk.stl b/locomotion/src/robot_simulation/robots/b1_description/meshes/trunk.stl similarity index 100% rename from locomotion/src/robots/b1_description/meshes/trunk.stl rename to locomotion/src/robot_simulation/robots/b1_description/meshes/trunk.stl diff --git a/locomotion/src/robots/b1_description/mujoco/assets/calf.mtl b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/calf.mtl similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/calf.mtl rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/calf.mtl diff --git a/locomotion/src/robots/b1_description/mujoco/assets/calf.obj b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/calf.obj similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/calf.obj rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/calf.obj diff --git a/locomotion/src/robots/b1_description/mujoco/assets/hip.mtl b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/hip.mtl similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/hip.mtl rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/hip.mtl diff --git a/locomotion/src/robots/b1_description/mujoco/assets/hip.obj b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/hip.obj similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/hip.obj rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/hip.obj diff --git a/locomotion/src/robots/b1_description/mujoco/assets/thigh.mtl b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh.mtl similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/thigh.mtl rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh.mtl diff --git a/locomotion/src/robots/b1_description/mujoco/assets/thigh.obj b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh.obj similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/thigh.obj rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh.obj diff --git a/locomotion/src/robots/b1_description/mujoco/assets/thigh/thigh.obj b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh/thigh.obj similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/thigh/thigh.obj rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh/thigh.obj diff --git a/locomotion/src/robots/b1_description/mujoco/assets/thigh_mirror.mtl b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh_mirror.mtl similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/thigh_mirror.mtl rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh_mirror.mtl diff --git a/locomotion/src/robots/b1_description/mujoco/assets/thigh_mirror.obj b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh_mirror.obj similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/thigh_mirror.obj rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/thigh_mirror.obj diff --git a/locomotion/src/robots/b1_description/mujoco/assets/trunk.mtl b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/trunk.mtl similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/trunk.mtl rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/trunk.mtl diff --git a/locomotion/src/robots/b1_description/mujoco/assets/trunk.obj b/locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/trunk.obj similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/assets/trunk.obj rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/assets/trunk.obj diff --git a/locomotion/src/robots/b1_description/mujoco/b1.xml b/locomotion/src/robot_simulation/robots/b1_description/mujoco/b1.xml similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/b1.xml rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/b1.xml diff --git a/locomotion/src/robots/b1_description/mujoco/b1_mjgen.xml b/locomotion/src/robot_simulation/robots/b1_description/mujoco/b1_mjgen.xml similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/b1_mjgen.xml rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/b1_mjgen.xml diff --git a/locomotion/src/robots/b1_description/mujoco/scene.xml b/locomotion/src/robot_simulation/robots/b1_description/mujoco/scene.xml similarity index 100% rename from locomotion/src/robots/b1_description/mujoco/scene.xml rename to locomotion/src/robot_simulation/robots/b1_description/mujoco/scene.xml diff --git a/locomotion/src/robots/b1_description/package.xml b/locomotion/src/robot_simulation/robots/b1_description/package.xml similarity index 100% rename from locomotion/src/robots/b1_description/package.xml rename to locomotion/src/robot_simulation/robots/b1_description/package.xml diff --git a/locomotion/src/robots/b1_description/urdf/b1_description.urdf b/locomotion/src/robot_simulation/robots/b1_description/urdf/b1_description.urdf similarity index 100% rename from locomotion/src/robots/b1_description/urdf/b1_description.urdf rename to locomotion/src/robot_simulation/robots/b1_description/urdf/b1_description.urdf diff --git a/locomotion/src/robots/b1_description/urdf/b1_description_mj.urdf b/locomotion/src/robot_simulation/robots/b1_description/urdf/b1_description_mj.urdf similarity index 100% rename from locomotion/src/robots/b1_description/urdf/b1_description_mj.urdf rename to locomotion/src/robot_simulation/robots/b1_description/urdf/b1_description_mj.urdf diff --git a/locomotion/src/robots/b1_description/xacro/const.xacro b/locomotion/src/robot_simulation/robots/b1_description/xacro/const.xacro similarity index 100% rename from locomotion/src/robots/b1_description/xacro/const.xacro rename to locomotion/src/robot_simulation/robots/b1_description/xacro/const.xacro diff --git a/locomotion/src/robots/b1_description/xacro/gazebo.xacro b/locomotion/src/robot_simulation/robots/b1_description/xacro/gazebo.xacro similarity index 100% rename from locomotion/src/robots/b1_description/xacro/gazebo.xacro rename to locomotion/src/robot_simulation/robots/b1_description/xacro/gazebo.xacro diff --git a/locomotion/src/robots/b1_description/xacro/leg.xacro b/locomotion/src/robot_simulation/robots/b1_description/xacro/leg.xacro similarity index 100% rename from locomotion/src/robots/b1_description/xacro/leg.xacro rename to locomotion/src/robot_simulation/robots/b1_description/xacro/leg.xacro diff --git a/locomotion/src/robots/b1_description/xacro/materials.xacro b/locomotion/src/robot_simulation/robots/b1_description/xacro/materials.xacro similarity index 100% rename from locomotion/src/robots/b1_description/xacro/materials.xacro rename to locomotion/src/robot_simulation/robots/b1_description/xacro/materials.xacro diff --git a/locomotion/src/robots/b1_description/xacro/robot.xacro b/locomotion/src/robot_simulation/robots/b1_description/xacro/robot.xacro similarity index 100% rename from locomotion/src/robots/b1_description/xacro/robot.xacro rename to locomotion/src/robot_simulation/robots/b1_description/xacro/robot.xacro diff --git a/locomotion/src/robots/b1_description/xacro/stairs.xacro b/locomotion/src/robot_simulation/robots/b1_description/xacro/stairs.xacro similarity index 100% rename from locomotion/src/robots/b1_description/xacro/stairs.xacro rename to locomotion/src/robot_simulation/robots/b1_description/xacro/stairs.xacro diff --git a/locomotion/src/robots/b1_description/xacro/transmission.xacro b/locomotion/src/robot_simulation/robots/b1_description/xacro/transmission.xacro similarity index 100% rename from locomotion/src/robots/b1_description/xacro/transmission.xacro rename to locomotion/src/robot_simulation/robots/b1_description/xacro/transmission.xacro diff --git a/locomotion/src/robots/convert_dae_to_stl_decimate.py b/locomotion/src/robot_simulation/robots/convert_dae_to_stl_decimate.py similarity index 100% rename from locomotion/src/robots/convert_dae_to_stl_decimate.py rename to locomotion/src/robot_simulation/robots/convert_dae_to_stl_decimate.py diff --git a/locomotion/src/robots/convert_stl_to_obj.py b/locomotion/src/robot_simulation/robots/convert_stl_to_obj.py similarity index 100% rename from locomotion/src/robots/convert_stl_to_obj.py rename to locomotion/src/robot_simulation/robots/convert_stl_to_obj.py diff --git a/locomotion/src/robots/go1_description/CMakeLists.txt b/locomotion/src/robot_simulation/robots/go1_description/CMakeLists.txt similarity index 100% rename from locomotion/src/robots/go1_description/CMakeLists.txt rename to locomotion/src/robot_simulation/robots/go1_description/CMakeLists.txt diff --git a/locomotion/src/robots/go1_description/config/robot_control.yaml b/locomotion/src/robot_simulation/robots/go1_description/config/robot_control.yaml similarity index 100% rename from locomotion/src/robots/go1_description/config/robot_control.yaml rename to locomotion/src/robot_simulation/robots/go1_description/config/robot_control.yaml diff --git a/locomotion/src/robots/go1_description/launch/check_joint.rviz b/locomotion/src/robot_simulation/robots/go1_description/launch/check_joint.rviz similarity index 100% rename from locomotion/src/robots/go1_description/launch/check_joint.rviz rename to locomotion/src/robot_simulation/robots/go1_description/launch/check_joint.rviz diff --git a/locomotion/src/robots/go1_description/launch/go1_rviz.launch b/locomotion/src/robot_simulation/robots/go1_description/launch/go1_rviz.launch similarity index 100% rename from locomotion/src/robots/go1_description/launch/go1_rviz.launch rename to locomotion/src/robot_simulation/robots/go1_description/launch/go1_rviz.launch diff --git a/locomotion/src/robots/go1_description/meshes/calf.dae b/locomotion/src/robot_simulation/robots/go1_description/meshes/calf.dae similarity index 100% rename from locomotion/src/robots/go1_description/meshes/calf.dae rename to locomotion/src/robot_simulation/robots/go1_description/meshes/calf.dae diff --git a/locomotion/src/robots/go1_description/meshes/depthCamera.dae b/locomotion/src/robot_simulation/robots/go1_description/meshes/depthCamera.dae similarity index 100% rename from locomotion/src/robots/go1_description/meshes/depthCamera.dae rename to locomotion/src/robot_simulation/robots/go1_description/meshes/depthCamera.dae diff --git a/locomotion/src/robots/go1_description/meshes/hip.dae b/locomotion/src/robot_simulation/robots/go1_description/meshes/hip.dae similarity index 100% rename from locomotion/src/robots/go1_description/meshes/hip.dae rename to locomotion/src/robot_simulation/robots/go1_description/meshes/hip.dae diff --git a/locomotion/src/robots/go1_description/meshes/thigh.dae b/locomotion/src/robot_simulation/robots/go1_description/meshes/thigh.dae similarity index 100% rename from locomotion/src/robots/go1_description/meshes/thigh.dae rename to locomotion/src/robot_simulation/robots/go1_description/meshes/thigh.dae diff --git a/locomotion/src/robots/go1_description/meshes/thigh_mirror.dae b/locomotion/src/robot_simulation/robots/go1_description/meshes/thigh_mirror.dae similarity index 100% rename from locomotion/src/robots/go1_description/meshes/thigh_mirror.dae rename to locomotion/src/robot_simulation/robots/go1_description/meshes/thigh_mirror.dae diff --git a/locomotion/src/robots/go1_description/meshes/trunk.dae b/locomotion/src/robot_simulation/robots/go1_description/meshes/trunk.dae similarity index 100% rename from locomotion/src/robots/go1_description/meshes/trunk.dae rename to locomotion/src/robot_simulation/robots/go1_description/meshes/trunk.dae diff --git a/locomotion/src/robots/go1_description/meshes/ultraSound.dae b/locomotion/src/robot_simulation/robots/go1_description/meshes/ultraSound.dae similarity index 100% rename from locomotion/src/robots/go1_description/meshes/ultraSound.dae rename to locomotion/src/robot_simulation/robots/go1_description/meshes/ultraSound.dae diff --git a/locomotion/src/robots/go1_description/package.xml b/locomotion/src/robot_simulation/robots/go1_description/package.xml similarity index 100% rename from locomotion/src/robots/go1_description/package.xml rename to locomotion/src/robot_simulation/robots/go1_description/package.xml diff --git a/locomotion/src/robots/go1_description/urdf/go1.urdf b/locomotion/src/robot_simulation/robots/go1_description/urdf/go1.urdf similarity index 100% rename from locomotion/src/robots/go1_description/urdf/go1.urdf rename to locomotion/src/robot_simulation/robots/go1_description/urdf/go1.urdf diff --git a/locomotion/src/robots/go1_description/xacro/const.xacro b/locomotion/src/robot_simulation/robots/go1_description/xacro/const.xacro similarity index 100% rename from locomotion/src/robots/go1_description/xacro/const.xacro rename to locomotion/src/robot_simulation/robots/go1_description/xacro/const.xacro diff --git a/locomotion/src/robots/go1_description/xacro/depthCamera.xacro b/locomotion/src/robot_simulation/robots/go1_description/xacro/depthCamera.xacro similarity index 100% rename from locomotion/src/robots/go1_description/xacro/depthCamera.xacro rename to locomotion/src/robot_simulation/robots/go1_description/xacro/depthCamera.xacro diff --git a/locomotion/src/robots/go1_description/xacro/gazebo.xacro b/locomotion/src/robot_simulation/robots/go1_description/xacro/gazebo.xacro similarity index 100% rename from locomotion/src/robots/go1_description/xacro/gazebo.xacro rename to locomotion/src/robot_simulation/robots/go1_description/xacro/gazebo.xacro diff --git a/locomotion/src/robots/go1_description/xacro/leg.xacro b/locomotion/src/robot_simulation/robots/go1_description/xacro/leg.xacro similarity index 100% rename from locomotion/src/robots/go1_description/xacro/leg.xacro rename to locomotion/src/robot_simulation/robots/go1_description/xacro/leg.xacro diff --git a/locomotion/src/robots/go1_description/xacro/materials.xacro b/locomotion/src/robot_simulation/robots/go1_description/xacro/materials.xacro similarity index 100% rename from locomotion/src/robots/go1_description/xacro/materials.xacro rename to locomotion/src/robot_simulation/robots/go1_description/xacro/materials.xacro diff --git a/locomotion/src/robots/go1_description/xacro/robot.xacro b/locomotion/src/robot_simulation/robots/go1_description/xacro/robot.xacro similarity index 100% rename from locomotion/src/robots/go1_description/xacro/robot.xacro rename to locomotion/src/robot_simulation/robots/go1_description/xacro/robot.xacro diff --git a/locomotion/src/robots/go1_description/xacro/transmission.xacro b/locomotion/src/robot_simulation/robots/go1_description/xacro/transmission.xacro similarity index 100% rename from locomotion/src/robots/go1_description/xacro/transmission.xacro rename to locomotion/src/robot_simulation/robots/go1_description/xacro/transmission.xacro diff --git a/locomotion/src/robots/go1_description/xacro/ultraSound.xacro b/locomotion/src/robot_simulation/robots/go1_description/xacro/ultraSound.xacro similarity index 100% rename from locomotion/src/robots/go1_description/xacro/ultraSound.xacro rename to locomotion/src/robot_simulation/robots/go1_description/xacro/ultraSound.xacro diff --git a/locomotion/src/robots/go2_description/CMakeLists.txt b/locomotion/src/robot_simulation/robots/go2_description/CMakeLists.txt similarity index 100% rename from locomotion/src/robots/go2_description/CMakeLists.txt rename to locomotion/src/robot_simulation/robots/go2_description/CMakeLists.txt diff --git a/locomotion/src/robots/go2_description/README.md b/locomotion/src/robot_simulation/robots/go2_description/README.md similarity index 100% rename from locomotion/src/robots/go2_description/README.md rename to locomotion/src/robot_simulation/robots/go2_description/README.md diff --git a/locomotion/src/robots/go2_description/config/joint_names_go2_description.yaml b/locomotion/src/robot_simulation/robots/go2_description/config/joint_names_go2_description.yaml similarity index 100% rename from locomotion/src/robots/go2_description/config/joint_names_go2_description.yaml rename to locomotion/src/robot_simulation/robots/go2_description/config/joint_names_go2_description.yaml diff --git a/locomotion/src/robots/go2_description/config/robot_control.yaml b/locomotion/src/robot_simulation/robots/go2_description/config/robot_control.yaml similarity index 100% rename from locomotion/src/robots/go2_description/config/robot_control.yaml rename to locomotion/src/robot_simulation/robots/go2_description/config/robot_control.yaml diff --git a/locomotion/src/robots/go2_description/dae/base.dae b/locomotion/src/robot_simulation/robots/go2_description/dae/base.dae similarity index 100% rename from locomotion/src/robots/go2_description/dae/base.dae rename to locomotion/src/robot_simulation/robots/go2_description/dae/base.dae diff --git a/locomotion/src/robots/go2_description/dae/calf.dae b/locomotion/src/robot_simulation/robots/go2_description/dae/calf.dae similarity index 100% rename from locomotion/src/robots/go2_description/dae/calf.dae rename to locomotion/src/robot_simulation/robots/go2_description/dae/calf.dae diff --git a/locomotion/src/robots/go2_description/dae/calf_mirror.dae b/locomotion/src/robot_simulation/robots/go2_description/dae/calf_mirror.dae similarity index 100% rename from locomotion/src/robots/go2_description/dae/calf_mirror.dae rename to locomotion/src/robot_simulation/robots/go2_description/dae/calf_mirror.dae diff --git a/locomotion/src/robots/go2_description/dae/foot.dae b/locomotion/src/robot_simulation/robots/go2_description/dae/foot.dae similarity index 100% rename from locomotion/src/robots/go2_description/dae/foot.dae rename to locomotion/src/robot_simulation/robots/go2_description/dae/foot.dae diff --git a/locomotion/src/robots/go2_description/dae/hip.dae b/locomotion/src/robot_simulation/robots/go2_description/dae/hip.dae similarity index 100% rename from locomotion/src/robots/go2_description/dae/hip.dae rename to locomotion/src/robot_simulation/robots/go2_description/dae/hip.dae diff --git a/locomotion/src/robots/go2_description/dae/thigh.dae b/locomotion/src/robot_simulation/robots/go2_description/dae/thigh.dae similarity index 100% rename from locomotion/src/robots/go2_description/dae/thigh.dae rename to locomotion/src/robot_simulation/robots/go2_description/dae/thigh.dae diff --git a/locomotion/src/robots/go2_description/dae/thigh_mirror.dae b/locomotion/src/robot_simulation/robots/go2_description/dae/thigh_mirror.dae similarity index 100% rename from locomotion/src/robots/go2_description/dae/thigh_mirror.dae rename to locomotion/src/robot_simulation/robots/go2_description/dae/thigh_mirror.dae diff --git a/locomotion/src/robots/go2_description/launch/check_joint.rviz b/locomotion/src/robot_simulation/robots/go2_description/launch/check_joint.rviz similarity index 100% rename from locomotion/src/robots/go2_description/launch/check_joint.rviz rename to locomotion/src/robot_simulation/robots/go2_description/launch/check_joint.rviz diff --git a/locomotion/src/robots/go2_description/launch/gazebo.launch b/locomotion/src/robot_simulation/robots/go2_description/launch/gazebo.launch similarity index 100% rename from locomotion/src/robots/go2_description/launch/gazebo.launch rename to locomotion/src/robot_simulation/robots/go2_description/launch/gazebo.launch diff --git a/locomotion/src/robots/go2_description/launch/go2_rviz.launch b/locomotion/src/robot_simulation/robots/go2_description/launch/go2_rviz.launch similarity index 100% rename from locomotion/src/robots/go2_description/launch/go2_rviz.launch rename to locomotion/src/robot_simulation/robots/go2_description/launch/go2_rviz.launch diff --git a/locomotion/src/robots/go2_description/meshes/base.stl b/locomotion/src/robot_simulation/robots/go2_description/meshes/base.stl similarity index 100% rename from locomotion/src/robots/go2_description/meshes/base.stl rename to locomotion/src/robot_simulation/robots/go2_description/meshes/base.stl diff --git a/locomotion/src/robots/go2_description/meshes/calf.dae b/locomotion/src/robot_simulation/robots/go2_description/meshes/calf.dae similarity index 100% rename from locomotion/src/robots/go2_description/meshes/calf.dae rename to locomotion/src/robot_simulation/robots/go2_description/meshes/calf.dae diff --git a/locomotion/src/robots/go2_description/meshes/calf.stl b/locomotion/src/robot_simulation/robots/go2_description/meshes/calf.stl similarity index 100% rename from locomotion/src/robots/go2_description/meshes/calf.stl rename to locomotion/src/robot_simulation/robots/go2_description/meshes/calf.stl diff --git a/locomotion/src/robots/go2_description/meshes/calf_mirror.dae b/locomotion/src/robot_simulation/robots/go2_description/meshes/calf_mirror.dae similarity index 100% rename from locomotion/src/robots/go2_description/meshes/calf_mirror.dae rename to locomotion/src/robot_simulation/robots/go2_description/meshes/calf_mirror.dae diff --git a/locomotion/src/robots/go2_description/meshes/calf_mirror.stl b/locomotion/src/robot_simulation/robots/go2_description/meshes/calf_mirror.stl similarity index 100% rename from locomotion/src/robots/go2_description/meshes/calf_mirror.stl rename to locomotion/src/robot_simulation/robots/go2_description/meshes/calf_mirror.stl diff --git a/locomotion/src/robots/go2_description/meshes/foot.dae b/locomotion/src/robot_simulation/robots/go2_description/meshes/foot.dae similarity index 100% rename from locomotion/src/robots/go2_description/meshes/foot.dae rename to locomotion/src/robot_simulation/robots/go2_description/meshes/foot.dae diff --git a/locomotion/src/robots/go2_description/meshes/foot.stl b/locomotion/src/robot_simulation/robots/go2_description/meshes/foot.stl similarity index 100% rename from locomotion/src/robots/go2_description/meshes/foot.stl rename to locomotion/src/robot_simulation/robots/go2_description/meshes/foot.stl diff --git a/locomotion/src/robots/go2_description/meshes/hip.dae b/locomotion/src/robot_simulation/robots/go2_description/meshes/hip.dae similarity index 100% rename from locomotion/src/robots/go2_description/meshes/hip.dae rename to locomotion/src/robot_simulation/robots/go2_description/meshes/hip.dae diff --git a/locomotion/src/robots/go2_description/meshes/hip.stl b/locomotion/src/robot_simulation/robots/go2_description/meshes/hip.stl similarity index 100% rename from locomotion/src/robots/go2_description/meshes/hip.stl rename to locomotion/src/robot_simulation/robots/go2_description/meshes/hip.stl diff --git a/locomotion/src/robots/go2_description/meshes/thigh.dae b/locomotion/src/robot_simulation/robots/go2_description/meshes/thigh.dae similarity index 100% rename from locomotion/src/robots/go2_description/meshes/thigh.dae rename to locomotion/src/robot_simulation/robots/go2_description/meshes/thigh.dae diff --git a/locomotion/src/robots/go2_description/meshes/thigh.stl b/locomotion/src/robot_simulation/robots/go2_description/meshes/thigh.stl similarity index 100% rename from locomotion/src/robots/go2_description/meshes/thigh.stl rename to locomotion/src/robot_simulation/robots/go2_description/meshes/thigh.stl diff --git a/locomotion/src/robots/go2_description/meshes/thigh_mirror.dae b/locomotion/src/robot_simulation/robots/go2_description/meshes/thigh_mirror.dae similarity index 100% rename from locomotion/src/robots/go2_description/meshes/thigh_mirror.dae rename to locomotion/src/robot_simulation/robots/go2_description/meshes/thigh_mirror.dae diff --git a/locomotion/src/robots/go2_description/meshes/thigh_mirror.stl b/locomotion/src/robot_simulation/robots/go2_description/meshes/thigh_mirror.stl similarity index 100% rename from locomotion/src/robots/go2_description/meshes/thigh_mirror.stl rename to locomotion/src/robot_simulation/robots/go2_description/meshes/thigh_mirror.stl diff --git a/locomotion/src/robots/go2_description/meshes/trunk.dae b/locomotion/src/robot_simulation/robots/go2_description/meshes/trunk.dae similarity index 100% rename from locomotion/src/robots/go2_description/meshes/trunk.dae rename to locomotion/src/robot_simulation/robots/go2_description/meshes/trunk.dae diff --git a/locomotion/src/robots/go2_description/meshes/trunk.stl b/locomotion/src/robot_simulation/robots/go2_description/meshes/trunk.stl similarity index 100% rename from locomotion/src/robots/go2_description/meshes/trunk.stl rename to locomotion/src/robot_simulation/robots/go2_description/meshes/trunk.stl diff --git a/locomotion/src/robots/go2_description/mujoco/LICENSE b/locomotion/src/robot_simulation/robots/go2_description/mujoco/LICENSE similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/LICENSE rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/LICENSE diff --git a/locomotion/src/robots/go2_description/mujoco/README.md b/locomotion/src/robot_simulation/robots/go2_description/mujoco/README.md similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/README.md rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/README.md diff --git a/locomotion/src/robots/go2_description/mujoco/assets.zip b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets.zip similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets.zip rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets.zip diff --git a/locomotion/src/robots/go2_description/mujoco/assets/base_0.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_0.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/base_0.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_0.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/base_1.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_1.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/base_1.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_1.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/base_2.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_2.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/base_2.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_2.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/base_3.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_3.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/base_3.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_3.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/base_4.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_4.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/base_4.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/base_4.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/calf_0.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/calf_0.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/calf_0.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/calf_0.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/calf_1.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/calf_1.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/calf_1.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/calf_1.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/calf_mirror_0.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/calf_mirror_0.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/calf_mirror_0.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/calf_mirror_0.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/calf_mirror_1.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/calf_mirror_1.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/calf_mirror_1.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/calf_mirror_1.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/foot.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/foot.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/foot.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/foot.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/hip_0.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/hip_0.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/hip_0.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/hip_0.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/hip_1.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/hip_1.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/hip_1.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/hip_1.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/thigh_0.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/thigh_0.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/thigh_0.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/thigh_0.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/thigh_1.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/thigh_1.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/thigh_1.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/thigh_1.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/thigh_mirror_0.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/thigh_mirror_0.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/thigh_mirror_0.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/thigh_mirror_0.obj diff --git a/locomotion/src/robots/go2_description/mujoco/assets/thigh_mirror_1.obj b/locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/thigh_mirror_1.obj similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/assets/thigh_mirror_1.obj rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/assets/thigh_mirror_1.obj diff --git a/locomotion/src/robots/go2_description/mujoco/go2.png b/locomotion/src/robot_simulation/robots/go2_description/mujoco/go2.png similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/go2.png rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/go2.png diff --git a/locomotion/src/robots/go2_description/mujoco/go2.xml b/locomotion/src/robot_simulation/robots/go2_description/mujoco/go2.xml similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/go2.xml rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/go2.xml diff --git a/locomotion/src/robots/go2_description/mujoco/go2_gen.xml b/locomotion/src/robot_simulation/robots/go2_description/mujoco/go2_gen.xml similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/go2_gen.xml rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/go2_gen.xml diff --git a/locomotion/src/robots/go2_description/mujoco/scene.xml b/locomotion/src/robot_simulation/robots/go2_description/mujoco/scene.xml similarity index 100% rename from locomotion/src/robots/go2_description/mujoco/scene.xml rename to locomotion/src/robot_simulation/robots/go2_description/mujoco/scene.xml diff --git a/locomotion/src/robots/go2_description/package.xml b/locomotion/src/robot_simulation/robots/go2_description/package.xml similarity index 100% rename from locomotion/src/robots/go2_description/package.xml rename to locomotion/src/robot_simulation/robots/go2_description/package.xml diff --git a/locomotion/src/robots/go2_description/urdf/Amended_collision_model.png b/locomotion/src/robot_simulation/robots/go2_description/urdf/Amended_collision_model.png similarity index 100% rename from locomotion/src/robots/go2_description/urdf/Amended_collision_model.png rename to locomotion/src/robot_simulation/robots/go2_description/urdf/Amended_collision_model.png diff --git a/locomotion/src/robots/go2_description/urdf/Normal_collision_model.png b/locomotion/src/robot_simulation/robots/go2_description/urdf/Normal_collision_model.png similarity index 100% rename from locomotion/src/robots/go2_description/urdf/Normal_collision_model.png rename to locomotion/src/robot_simulation/robots/go2_description/urdf/Normal_collision_model.png diff --git a/locomotion/src/robots/go2_description/urdf/go2_description.urdf b/locomotion/src/robot_simulation/robots/go2_description/urdf/go2_description.urdf similarity index 100% rename from locomotion/src/robots/go2_description/urdf/go2_description.urdf rename to locomotion/src/robot_simulation/robots/go2_description/urdf/go2_description.urdf diff --git a/locomotion/src/robots/go2_description/urdf/go2_description2.urdf b/locomotion/src/robot_simulation/robots/go2_description/urdf/go2_description2.urdf similarity index 100% rename from locomotion/src/robots/go2_description/urdf/go2_description2.urdf rename to locomotion/src/robot_simulation/robots/go2_description/urdf/go2_description2.urdf diff --git a/locomotion/src/robots/go2_description/xacro/const.xacro b/locomotion/src/robot_simulation/robots/go2_description/xacro/const.xacro similarity index 100% rename from locomotion/src/robots/go2_description/xacro/const.xacro rename to locomotion/src/robot_simulation/robots/go2_description/xacro/const.xacro diff --git a/locomotion/src/robots/go2_description/xacro/gazebo.xacro b/locomotion/src/robot_simulation/robots/go2_description/xacro/gazebo.xacro similarity index 100% rename from locomotion/src/robots/go2_description/xacro/gazebo.xacro rename to locomotion/src/robot_simulation/robots/go2_description/xacro/gazebo.xacro diff --git a/locomotion/src/robots/go2_description/xacro/leg.xacro b/locomotion/src/robot_simulation/robots/go2_description/xacro/leg.xacro similarity index 100% rename from locomotion/src/robots/go2_description/xacro/leg.xacro rename to locomotion/src/robot_simulation/robots/go2_description/xacro/leg.xacro diff --git a/locomotion/src/robots/go2_description/xacro/materials.xacro b/locomotion/src/robot_simulation/robots/go2_description/xacro/materials.xacro similarity index 100% rename from locomotion/src/robots/go2_description/xacro/materials.xacro rename to locomotion/src/robot_simulation/robots/go2_description/xacro/materials.xacro diff --git a/locomotion/src/robots/go2_description/xacro/robot.xacro b/locomotion/src/robot_simulation/robots/go2_description/xacro/robot.xacro similarity index 100% rename from locomotion/src/robots/go2_description/xacro/robot.xacro rename to locomotion/src/robot_simulation/robots/go2_description/xacro/robot.xacro diff --git a/locomotion/src/robots/go2_description/xacro/transmission.xacro b/locomotion/src/robot_simulation/robots/go2_description/xacro/transmission.xacro similarity index 100% rename from locomotion/src/robots/go2_description/xacro/transmission.xacro rename to locomotion/src/robot_simulation/robots/go2_description/xacro/transmission.xacro diff --git a/locomotion/src/robots/m2_description/CMakeLists.txt b/locomotion/src/robot_simulation/robots/m2_description/CMakeLists.txt similarity index 100% rename from locomotion/src/robots/m2_description/CMakeLists.txt rename to locomotion/src/robot_simulation/robots/m2_description/CMakeLists.txt diff --git a/locomotion/src/robots/m2_description/config/joint_names_SVANM2_URDF.yaml b/locomotion/src/robot_simulation/robots/m2_description/config/joint_names_SVANM2_URDF.yaml similarity index 100% rename from locomotion/src/robots/m2_description/config/joint_names_SVANM2_URDF.yaml rename to locomotion/src/robot_simulation/robots/m2_description/config/joint_names_SVANM2_URDF.yaml diff --git a/locomotion/src/robots/m2_description/config/joint_names_SVAN_URDF4.yaml b/locomotion/src/robot_simulation/robots/m2_description/config/joint_names_SVAN_URDF4.yaml similarity index 100% rename from locomotion/src/robots/m2_description/config/joint_names_SVAN_URDF4.yaml rename to locomotion/src/robot_simulation/robots/m2_description/config/joint_names_SVAN_URDF4.yaml diff --git a/locomotion/src/robots/m2_description/launch/display.launch b/locomotion/src/robot_simulation/robots/m2_description/launch/display.launch similarity index 100% rename from locomotion/src/robots/m2_description/launch/display.launch rename to locomotion/src/robot_simulation/robots/m2_description/launch/display.launch diff --git a/locomotion/src/robots/m2_description/launch/gazebo.launch b/locomotion/src/robot_simulation/robots/m2_description/launch/gazebo.launch similarity index 100% rename from locomotion/src/robots/m2_description/launch/gazebo.launch rename to locomotion/src/robot_simulation/robots/m2_description/launch/gazebo.launch diff --git a/locomotion/src/robots/m2_description/meshes/BASE.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/BASE.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/BASE.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/BASE.STL diff --git a/locomotion/src/robots/m2_description/meshes/Empty_Link1.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/Empty_Link1.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/Empty_Link1.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/Empty_Link1.STL diff --git a/locomotion/src/robots/m2_description/meshes/Empty_Link2.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/Empty_Link2.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/Empty_Link2.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/Empty_Link2.STL diff --git a/locomotion/src/robots/m2_description/meshes/Empty_Link3.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/Empty_Link3.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/Empty_Link3.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/Empty_Link3.STL diff --git a/locomotion/src/robots/m2_description/meshes/Empty_Link4.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/Empty_Link4.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/Empty_Link4.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/Empty_Link4.STL diff --git a/locomotion/src/robots/m2_description/meshes/LF_HIP.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/LF_HIP.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/LF_HIP.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/LF_HIP.STL diff --git a/locomotion/src/robots/m2_description/meshes/LF_SHANK.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/LF_SHANK.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/LF_SHANK.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/LF_SHANK.STL diff --git a/locomotion/src/robots/m2_description/meshes/LF_THIGH.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/LF_THIGH.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/LF_THIGH.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/LF_THIGH.STL diff --git a/locomotion/src/robots/m2_description/meshes/LH_HIP.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/LH_HIP.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/LH_HIP.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/LH_HIP.STL diff --git a/locomotion/src/robots/m2_description/meshes/LH_SHANK.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/LH_SHANK.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/LH_SHANK.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/LH_SHANK.STL diff --git a/locomotion/src/robots/m2_description/meshes/LH_THIGH.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/LH_THIGH.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/LH_THIGH.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/LH_THIGH.STL diff --git a/locomotion/src/robots/m2_description/meshes/RF_HIP.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/RF_HIP.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/RF_HIP.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/RF_HIP.STL diff --git a/locomotion/src/robots/m2_description/meshes/RF_SHANK.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/RF_SHANK.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/RF_SHANK.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/RF_SHANK.STL diff --git a/locomotion/src/robots/m2_description/meshes/RF_THIGH.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/RF_THIGH.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/RF_THIGH.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/RF_THIGH.STL diff --git a/locomotion/src/robots/m2_description/meshes/RH_HIP.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/RH_HIP.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/RH_HIP.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/RH_HIP.STL diff --git a/locomotion/src/robots/m2_description/meshes/RH_SHANK.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/RH_SHANK.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/RH_SHANK.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/RH_SHANK.STL diff --git a/locomotion/src/robots/m2_description/meshes/RH_THIGH.STL b/locomotion/src/robot_simulation/robots/m2_description/meshes/RH_THIGH.STL similarity index 100% rename from locomotion/src/robots/m2_description/meshes/RH_THIGH.STL rename to locomotion/src/robot_simulation/robots/m2_description/meshes/RH_THIGH.STL diff --git a/locomotion/src/robots/m2_description/mujoco/assets/BASE.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/BASE.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/BASE.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/BASE.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/BASE.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/BASE.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/BASE.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/BASE.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link1.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link1.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link1.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link1.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link1.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link1.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link1.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link1.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link2.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link2.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link2.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link2.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link2.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link2.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link2.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link2.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link3.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link3.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link3.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link3.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link3.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link3.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link3.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link3.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link3/Empty_Link3.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link3/Empty_Link3.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link3/Empty_Link3.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link3/Empty_Link3.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link4.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link4.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link4.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link4.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/Empty_Link4.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link4.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/Empty_Link4.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/Empty_Link4.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LF_HIP.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_HIP.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LF_HIP.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_HIP.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LF_HIP.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_HIP.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LF_HIP.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_HIP.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LF_SHANK.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_SHANK.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LF_SHANK.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_SHANK.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LF_SHANK.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_SHANK.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LF_SHANK.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_SHANK.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LF_THIGH.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_THIGH.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LF_THIGH.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_THIGH.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LF_THIGH.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_THIGH.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LF_THIGH.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LF_THIGH.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LH_HIP.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_HIP.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LH_HIP.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_HIP.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LH_HIP.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_HIP.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LH_HIP.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_HIP.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LH_SHANK.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_SHANK.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LH_SHANK.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_SHANK.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LH_SHANK.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_SHANK.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LH_SHANK.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_SHANK.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LH_THIGH.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_THIGH.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LH_THIGH.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_THIGH.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/LH_THIGH.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_THIGH.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/LH_THIGH.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/LH_THIGH.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RF_HIP.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_HIP.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RF_HIP.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_HIP.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RF_HIP.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_HIP.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RF_HIP.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_HIP.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RF_SHANK.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_SHANK.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RF_SHANK.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_SHANK.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RF_SHANK.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_SHANK.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RF_SHANK.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_SHANK.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RF_THIGH.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_THIGH.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RF_THIGH.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_THIGH.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RF_THIGH.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_THIGH.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RF_THIGH.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RF_THIGH.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RH_HIP.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_HIP.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RH_HIP.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_HIP.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RH_HIP.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_HIP.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RH_HIP.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_HIP.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RH_SHANK.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_SHANK.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RH_SHANK.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_SHANK.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RH_SHANK.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_SHANK.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RH_SHANK.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_SHANK.obj diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RH_THIGH.mtl b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_THIGH.mtl similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RH_THIGH.mtl rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_THIGH.mtl diff --git a/locomotion/src/robots/m2_description/mujoco/assets/RH_THIGH.obj b/locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_THIGH.obj similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/assets/RH_THIGH.obj rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/assets/RH_THIGH.obj diff --git a/locomotion/src/robots/m2_description/mujoco/m2.xml b/locomotion/src/robot_simulation/robots/m2_description/mujoco/m2.xml similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/m2.xml rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/m2.xml diff --git a/locomotion/src/robots/m2_description/mujoco/m2_model.xml b/locomotion/src/robot_simulation/robots/m2_description/mujoco/m2_model.xml similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/m2_model.xml rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/m2_model.xml diff --git a/locomotion/src/robots/m2_description/mujoco/scene.xml b/locomotion/src/robot_simulation/robots/m2_description/mujoco/scene.xml similarity index 100% rename from locomotion/src/robots/m2_description/mujoco/scene.xml rename to locomotion/src/robot_simulation/robots/m2_description/mujoco/scene.xml diff --git a/locomotion/src/robots/m2_description/package.xml b/locomotion/src/robot_simulation/robots/m2_description/package.xml similarity index 100% rename from locomotion/src/robots/m2_description/package.xml rename to locomotion/src/robot_simulation/robots/m2_description/package.xml diff --git a/locomotion/src/robots/m2_description/urdf/SVANM2_URDF.csv b/locomotion/src/robot_simulation/robots/m2_description/urdf/SVANM2_URDF.csv similarity index 100% rename from locomotion/src/robots/m2_description/urdf/SVANM2_URDF.csv rename to locomotion/src/robot_simulation/robots/m2_description/urdf/SVANM2_URDF.csv diff --git a/locomotion/src/robots/m2_description/urdf/m2_description.urdf b/locomotion/src/robot_simulation/robots/m2_description/urdf/m2_description.urdf similarity index 100% rename from locomotion/src/robots/m2_description/urdf/m2_description.urdf rename to locomotion/src/robot_simulation/robots/m2_description/urdf/m2_description.urdf diff --git a/locomotion/src/robots/m2_description/urdf/m2_description2.urdf b/locomotion/src/robot_simulation/robots/m2_description/urdf/m2_description2.urdf similarity index 100% rename from locomotion/src/robots/m2_description/urdf/m2_description2.urdf rename to locomotion/src/robot_simulation/robots/m2_description/urdf/m2_description2.urdf diff --git a/locomotion/src/robots/m2_description/urdf/svanM2.csv b/locomotion/src/robot_simulation/robots/m2_description/urdf/svanM2.csv similarity index 100% rename from locomotion/src/robots/m2_description/urdf/svanM2.csv rename to locomotion/src/robot_simulation/robots/m2_description/urdf/svanM2.csv diff --git a/locomotion/src/robots/m2_description/xacro/const.xacro b/locomotion/src/robot_simulation/robots/m2_description/xacro/const.xacro similarity index 100% rename from locomotion/src/robots/m2_description/xacro/const.xacro rename to locomotion/src/robot_simulation/robots/m2_description/xacro/const.xacro diff --git a/locomotion/src/robots/m2_description/xacro/robot.xacro b/locomotion/src/robot_simulation/robots/m2_description/xacro/robot.xacro similarity index 100% rename from locomotion/src/robots/m2_description/xacro/robot.xacro rename to locomotion/src/robot_simulation/robots/m2_description/xacro/robot.xacro diff --git a/locomotion/src/robot_simulation/src/MJSim.cpp b/locomotion/src/robot_simulation/src/MJSim.cpp deleted file mode 100644 index 8060d83..0000000 --- a/locomotion/src/robot_simulation/src/MJSim.cpp +++ /dev/null @@ -1,200 +0,0 @@ -#include "MJSim.hpp" - -MJSim::MJSim() : m_name("go2") { - // Load and compile model - init_sim(); -} - -MJSim::~MJSim() { - // free visualization storage - mjv_freeScene(&scn); - mjr_freeContext(&con); - - // free MuJoCo model and data, deactivate - mj_deleteData(d); - mj_deleteModel(m); - - // terminate GLFW (crashes with Linux NVidia drivers) - #if defined(__APPLE__) || defined(_WIN32) - glfwTerminate(); - #endif -} - -void MJSim::CustomController(const mjModel* model, mjData* data) { - // controller with sensor readings - if (previous_time == 0) - { - previous_time = data->time; - return; - } - if (data->time - last_update > 1.0/ctrl_update_freq) - { - // Get the velocity of joint directly from MuJoCo? - mjtNum vel = (data->sensordata[0] - position_history)/(d->time-previous_time); - - for (int i = 0; i < 12; ++i) { - float dtheta = joint_command_data.q(i) - sensor_data.qd(i); - float dtheta_d = joint_command_data.qd(i) - sensor_data.qd(i); - data->ctrl[i] = joint_command_data.kp(i) * dtheta * joint_command_data.kd(i) * dtheta_d; - } - last_update = data->time; - position_history = data->sensordata[0]; - previous_time = data->time; - } -} -void MJSim::Run() { - // use the first while condition if you want to simulate for a period. - // while( !glfwWindowShouldClose(window) and d->time-timezero < 1.5) - while( !glfwWindowShouldClose(window)) - { - // advance interactive simulation for 1/60 sec - // Assuming MuJoCo can simulate faster than real-time, which it usually can, - // this loop will finish on time for the next frame to be rendered at 60 fps. - // Otherwise add a cpu timer and exit this loop when it is time to render. - mjtNum simstart = d->time; - while( d->time - simstart < 1.0/60.0 ) { - UpdateSensorData(); - mj_step(m, d); - } - - // 15 ms is a little smaller than 60 Hz. - std::this_thread::sleep_for(std::chrono::milliseconds(15)); - // get framebuffer viewport - mjrRect viewport = {0, 0, 0, 0}; - glfwGetFramebufferSize(window, &viewport.width, &viewport.height); - - // update scene and render - mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn); - mjr_render(viewport, &scn, &con); - - // swap OpenGL buffers (blocking call due to v-sync) - glfwSwapBuffers(window); - - // process pending GUI events, call GLFW callbacks - glfwPollEvents(); - - } -} - -void MJSim::init_sim() { - std::string model_file = "../../robots/" + m_name + "_description/mujoco/" + m_name + ".xml"; - - // load and compile model - char error[1000] = "Could not load binary model"; - if (std::strlen(model_file.c_str())>4 && !std::strcmp(model_file.c_str() + std::strlen(model_file.c_str())-4, ".mjb")) { - m = mj_loadModel(model_file.c_str(), 0); - } else { - m = mj_loadXML(model_file.c_str(), 0, error, 1000); - } - if (!m) { - mju_error("Load model error: %s", error); - } - // make data - d = mj_makeData(m); - - // init GLFW - if (!glfwInit()) { - mju_error("Could not initialize GLFW"); - } - - // create window, make OpenGL context current, request v-sync - window = glfwCreateWindow(1200, 900, "Demo", NULL, NULL); - glfwMakeContextCurrent(window); - glfwSwapInterval(1); - - // initialize visualization data structures - mjv_defaultCamera(&cam); - mjv_defaultOption(&opt); - mjv_defaultScene(&scn); - mjr_defaultContext(&con); - - // create scene and context - mjv_makeScene(m, &scn, 2000); - mjr_makeContext(m, &con, mjFONTSCALE_150); - - // install GLFW mouse and keyboard callbacks - glfwSetKeyCallback(window, keyboard); - glfwSetCursorPosCallback(window, mouse_move); - glfwSetMouseButtonCallback(window, mouse_button); - glfwSetScrollCallback(window, scroll); - - // install control callback - mjcb_control = this->CustomController; - - // Initialize DOFs - // d->qpos[0] = 1.57; - - // run main loop, target real-time simulation and 60 fps rendering - mjtNum timezero = d->time; - double_t update_rate = 0.01; - - // making sure the first time step updates the ctrl previous_time - last_update = timezero-1.0/ctrl_update_freq; -} - -// keyboard callback -void MJSim::keyboard(GLFWwindow* window, int key, int scancode, int act, int mods) { - // backspace: reset simulation - if (act==GLFW_PRESS && key==GLFW_KEY_BACKSPACE) { - mj_resetData(m, d); - mj_forward(m, d); - } -} - -// mouse button callback -void MJSim::mouse_button(GLFWwindow* window, int button, int act, int mods) { - // update button state - button_left = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT)==GLFW_PRESS); - button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE)==GLFW_PRESS); - button_right = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT)==GLFW_PRESS); - - // update mouse position - glfwGetCursorPos(window, &lastx, &lasty); -} - -// mouse move callback -void MJSim::mouse_move(GLFWwindow* window, double xpos, double ypos) { - // no buttons down: nothing to do - if (!button_left && !button_middle && !button_right) { - return; - } - - // compute mouse displacement, save - double dx = xpos - lastx; - double dy = ypos - lasty; - lastx = xpos; - lasty = ypos; - - // get current window size - int width, height; - glfwGetWindowSize(window, &width, &height); - - // get shift key state - bool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS || - glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS); - - // determine action based on mouse button - mjtMouse action; - if (button_right) { - action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V; - } else if (button_left) { - action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V; - } else { - action = mjMOUSE_ZOOM; - } - - // move camera - mjv_moveCamera(m, action, dx/height, dy/height, &scn, &cam); -} - -// scroll callback -void MJSim::scroll(GLFWwindow* window, double xoffset, double yoffset) { - // emulate vertical mouse motion = 5% of window height - mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05*yoffset, &scn, &cam); -} - -void MJSim::UpdateSensorData() { - for (int i = 0; i < 12; ++i) { - sensor_data.q(i) = d -> sensordata[i]; - } -} \ No newline at end of file diff --git a/locomotion/src/robot_simulation/src/communication/CommunicationManager.cpp b/locomotion/src/robot_simulation/src/communication/CommunicationManager.cpp new file mode 100644 index 0000000..6be0604 --- /dev/null +++ b/locomotion/src/robot_simulation/src/communication/CommunicationManager.cpp @@ -0,0 +1,100 @@ +#include "CommunicationManager.hpp" + +CommunicationManager::CommunicationManager() : m_mode(DATA_ACCESS_MODE::PLANT) { +} +CommunicationManager::CommunicationManager(const DATA_ACCESS_MODE& mode) : m_name("svan"), m_mode(mode) { +} + +CommunicationManager::CommunicationManager(const std::string& name, const DATA_ACCESS_MODE& mode) : m_name(name), m_mode(mode) { +} + +void CommunicationManager::writeSensorData(const QuadrupedSensorData& sensor_data) { + if (m_mode == DATA_ACCESS_MODE::EXECUTOR) { + std::cout << "Access mode set to only read sensor data. Cannot Write. Operation failed.\n"; + return; + } + if (m_sensor_data_ptr == NULL) { + std::cout << "Could not get the address to write sensor data...\n"; + return; + } + m_sensor_data_ptr->copy(sensor_data); +} + +void CommunicationManager::writeMeasurementData(const QuadrupedMeasurementData& measure_data) { + if (m_mode == DATA_ACCESS_MODE::EXECUTOR) { + std::cout << "Access mode set to only read measurement data. Cannot Write. Operation failed.\n"; + return; + } + if (m_measurement_data_ptr == NULL) { + std::cout << "Could not get the address to write measurement data...\n"; + return; + } + m_measurement_data_ptr->copy(measure_data); +} + +void CommunicationManager::writeCommandData(const QuadrupedCommandData& cmd_data) { + if (m_mode == DATA_ACCESS_MODE::PLANT) { + std::cout << "Access mode set to only read command data. Cannot Write. Operation failed.\n"; + return; + } + if (m_joint_command_data_ptr == NULL) { + std::cout << "Could not get the address to write command data...\n"; + return; + } + m_joint_command_data_ptr->copy(cmd_data); +} + +void CommunicationManager::writeEstimationData(const QuadrupedEstimationData& est_data) { + if (m_mode == DATA_ACCESS_MODE::PLANT) { + std::cout << "Access mode set to only read command data. Cannot Write. Operation failed.\n"; + return; + } + if (m_estimation_data_ptr == NULL) { + std::cout << "Could not get the address to write estimation data...\n"; + return; + } + m_estimation_data_ptr->copy(est_data); +} + +void CommunicationManager::getSensorData(QuadrupedSensorData& sensor_data) { + // if (m_mode == DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) { + // std::cout << "Access mode set to only write sensor data. Cannot read. Operation failed.\n"; + // return; + // } + if (m_sensor_data_ptr == NULL) { + std::cout << "Memory not initialized. Cannot read.\n"; + } + sensor_data.copy(*m_sensor_data_ptr); +} + +void CommunicationManager::getMeasurememtData(QuadrupedMeasurementData& measure_data) { + // if (m_mode == DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) { + // std::cout << "Access mode set to only write measurement data. Cannot read. Operation failed.\n"; + // return; + // } + if (m_measurement_data_ptr == NULL) { + std::cout << "Memory not initialized. Cannot read.\n"; + } + measure_data.copy(*m_measurement_data_ptr); +} + +void CommunicationManager::getCommandData(QuadrupedCommandData& cmd_data) { + // if (m_mode == DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) { + // std::cout << "Access mode set to only write command data. Cannot read. Operation failed.\n"; + // return; + // } + if (m_joint_command_data_ptr == NULL) { + std::cout << "Memory not initialized. Cannot read.\n"; + } + cmd_data.copy(*m_joint_command_data_ptr); +} +void CommunicationManager::getEstimationData(QuadrupedEstimationData& est_data) { + // if (m_mode == DATA_ACCESS_MODE::EXECUTOR_TO_PLANT) { + // std::cout << "Access mode set to only write command data. Cannot read. Operation failed.\n"; + // return; + // } + if (m_estimation_data_ptr == NULL) { + std::cout << "Memory not initialized. Cannot read.\n"; + } + est_data.copy(*m_estimation_data_ptr); +} \ No newline at end of file diff --git a/locomotion/src/robot_simulation/src/communication/QuadROSComm.cpp b/locomotion/src/robot_simulation/src/communication/QuadROSComm.cpp new file mode 100644 index 0000000..d2a8a34 --- /dev/null +++ b/locomotion/src/robot_simulation/src/communication/QuadROSComm.cpp @@ -0,0 +1,248 @@ +#include "QuadROSComm.hpp" + +QuadROSComm::QuadROSComm(const DATA_ACCESS_MODE& mode) : m_name("svan"), rclcpp::Node("svan"), CommunicationManager(mode) { + InitClass(); +} + +QuadROSComm::QuadROSComm(const std::string& name, const DATA_ACCESS_MODE& mode) : m_name(name), rclcpp::Node(name), CommunicationManager(name, mode) { + InitClass(); +} + +QuadROSComm::~QuadROSComm() { + rclcpp::shutdown(); +} + +void QuadROSComm::InitClass() { + m_dt = 1./m_loop_rate; + + auto pub_timer = std::chrono::duration(m_dt); + m_timer = this->create_wall_timer(pub_timer, std::bind(&QuadROSComm::timer_cb, this)); + + if (m_mode == DATA_ACCESS_MODE::EXECUTOR) { + m_joint_state_pub = this->create_publisher("/" + m_name + "/joint_cmd", 1); + + m_joint_state_sub = this->create_subscription( + "/" + m_name + "/joint_state", + 1, + std::bind(&QuadROSComm::get_joint_state_cb, this, std::placeholders::_1) + ); + m_imu_sub = this->create_subscription( + "/" + m_name + "/imu", + 1, + std::bind(&QuadROSComm::get_imu_cb, this, std::placeholders::_1) + ); + m_odom_sub = this->create_subscription( + "/" + m_name + "/odom", + 1, + std::bind(&QuadROSComm::get_odom_cb, this, std::placeholders::_1) + ); + for (int i = 0; i < 4; ++i) { + m_est_cs_pub[i] = this->create_publisher( + "/" + m_name + "/estimation/contact_state/cs" + std::to_string(i), + 1 + ); + m_est_pc_pub[i] = this->create_publisher( + "/" + m_name + "/estimation/contact_probability/pc" + std::to_string(i), + 1 + ); + est_cs_data[i] = std_msgs::msg::Float32(); + est_pc_data[i] = std_msgs::msg::Float32(); + est_cs_data[i].data = 0; + est_pc_data[i].data = 0; + } + for (int i = 0; i < 3; ++i) { + m_est_base_position_pub[i] = this->create_publisher( + "/" + m_name + "/estimation/base_position/rB" + std::to_string(i), + 1 + ); + m_est_base_velocity_pub[i] = this->create_publisher( + "/" + m_name + "/estimation/base_velocity/vB" + std::to_string(i), + 1 + ); + est_base_pos_data[i] = std_msgs::msg::Float32(); + est_base_vel_data[i] = std_msgs::msg::Float32(); + est_base_pos_data[i].data = 0; + est_base_vel_data[i].data = 0; + } + for (int i = 0; i < 12; ++i) { + // m_contact_forces_pub[i] = this->create_publisher("/" + m_name + "/sim/contact_forces_" + std::to_string(i), 1); + m_estimated_fc_pub[i] = this->create_publisher("/" + m_name + "/estimation/contact_forces_" + std::to_string(i), 1); + } + } else if (m_mode == DATA_ACCESS_MODE::PLANT) { + m_joint_state_pub = this->create_publisher("/" + m_name + "/joint_state", 1); + m_imu_pub = this->create_publisher("/" + m_name + "/imu", 1); + m_odom_pub = this->create_publisher("/" + m_name + "/odom", 1); + m_base_position_pub = this->create_publisher("/" + m_name + "/sim/base_position", 1); + m_base_vel_pub = this->create_publisher("/" + m_name + "/sim/base_velocity", 1); + m_base_acc_pub = this->create_publisher("/" + m_name + "/sim/base_acceleration", 1); + for (int i = 0; i < 12; ++i) { + m_contact_forces_pub[i] = this->create_publisher("/" + m_name + "/sim/contact_forces_" + std::to_string(i), 1); + // m_estimated_fc_pub[i] = this->create_publisher("/" + m_name + "/estimation/contact_forces_" + std::to_string(i), 1); + } + + // auto pub_timer = std::chrono::duration(m_dt); + // m_timer = this->create_wall_timer(pub_timer, std::bind(&QuadROSComm::timer_cb, this)); + + m_joint_cmd_sub = this->create_subscription( + "/" + m_name + "/joint_cmd", + 1, + std::bind(&QuadROSComm::get_joint_cmd_cb, this, std::placeholders::_1) + ); + + + } + + js_data = sensor_msgs::msg::JointState(); + js_data.position.clear(); + js_data.velocity.clear(); + js_data.effort.clear(); + for (int i = 0; i < 12; ++i) { + js_data.position.push_back(0); + js_data.velocity.push_back(0); + js_data.effort.push_back(0); + } + + // js_data = sensor_msgs::msg::JointState(); + imu_data = sensor_msgs::msg::Imu(); + odom_data = nav_msgs::msg::Odometry(); + base_position_data = geometry_msgs::msg::Point(); + base_velocity_data = geometry_msgs::msg::Twist(); + base_acceleration_data = geometry_msgs::msg::Twist(); + for (int i = 0; i < 12; ++i) { + contact_forces_data[i] = std_msgs::msg::Float32(); + contact_forces_data[i].data = 0; + estimated_fc_data[i] = std_msgs::msg::Float32(); + estimated_fc_data[i].data = 0; + } + + // js_data.position.clear(); + // js_data.velocity.clear(); + // js_data.effort.clear(); + // for (int i = 0; i < 12; ++i) { + // js_data.position.push_back(0); + // js_data.velocity.push_back(0); + // js_data.effort.push_back(0); + // } +} + +void QuadROSComm::get_joint_state_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const { + for (int i = 0; i < 12; ++i) { + m_sensor_data_ptr -> q(i) = msg -> position[i]; + m_sensor_data_ptr -> qd(i) = msg -> velocity[i]; + m_sensor_data_ptr -> tau(i) = msg -> effort[i]; + } +} + +void QuadROSComm::get_imu_cb(const sensor_msgs::msg::Imu::SharedPtr msg) const { + m_sensor_data_ptr -> quat(0) = msg->orientation.x; + m_sensor_data_ptr -> quat(1) = msg->orientation.y; + m_sensor_data_ptr -> quat(2) = msg->orientation.z; + m_sensor_data_ptr -> quat(3) = msg->orientation.w; + + m_sensor_data_ptr -> w_B(0) = msg->angular_velocity.x; + m_sensor_data_ptr -> w_B(1) = msg->angular_velocity.y; + m_sensor_data_ptr -> w_B(2) = msg->angular_velocity.z; + + m_sensor_data_ptr -> a_B(0) = msg->linear_acceleration.x; + m_sensor_data_ptr -> a_B(1) = msg->linear_acceleration.y; + m_sensor_data_ptr -> a_B(2) = msg->linear_acceleration.z; + + // TODO: populate a_W and w_W as well +} + +void QuadROSComm::get_odom_cb(const nav_msgs::msg::Odometry::SharedPtr msg) const { + // TODO: Use Odom data as required. Maybe extend sensor_data to have odom data? +} + +void QuadROSComm::get_joint_cmd_cb(const sensor_msgs::msg::JointState::SharedPtr msg) const { + for (int i = 0; i < 12; ++i) { + m_joint_command_data_ptr -> q(i) = msg -> position[i]; + m_joint_command_data_ptr -> qd(i) = msg -> velocity[i]; + m_joint_command_data_ptr -> tau(i) = msg -> effort[i]; + } +} + +void QuadROSComm::timer_cb() { + if (m_mode == DATA_ACCESS_MODE::EXECUTOR) { + for (int i = 0; i < 12; ++i) { + js_data.position[i] = m_joint_command_data_ptr -> q(i); + js_data.velocity[i] = m_joint_command_data_ptr -> qd(i); + js_data.effort[i] = m_joint_command_data_ptr -> tau(i); + } + for (int i = 0; i < 4; ++i) { + est_cs_data[i].data = m_estimation_data_ptr -> cs(i); + est_pc_data[i].data = m_estimation_data_ptr -> pc(i); + m_est_cs_pub[i]->publish(est_cs_data[i]); + m_est_pc_pub[i]->publish(est_pc_data[i]); + } + for (int i = 0; i < 3; ++i) { + est_base_pos_data[i].data = m_estimation_data_ptr -> rB(i); + est_base_vel_data[i].data = m_estimation_data_ptr -> vB(i); + m_est_base_position_pub[i]->publish(est_base_pos_data[i]); + m_est_base_velocity_pub[i]->publish(est_base_vel_data[i]); + } + + for (int i = 0; i < 12; ++i) { + estimated_fc_data[i].data = m_measurement_data_ptr -> estimated_contact_force(i); + m_estimated_fc_pub[i]->publish(estimated_fc_data[i]); + } + + m_joint_state_pub->publish(js_data); + } else if (m_mode == DATA_ACCESS_MODE::PLANT) { + for (int i = 0; i < 12; ++i) { + js_data.position[i] = m_sensor_data_ptr -> q(i); + js_data.velocity[i] = m_sensor_data_ptr -> qd(i); + js_data.effort[i] = m_sensor_data_ptr -> tau(i); + } + // Populate imu_data + imu_data.orientation.w = m_sensor_data_ptr -> quat(3); + imu_data.orientation.x = m_sensor_data_ptr -> quat(0); + imu_data.orientation.y = m_sensor_data_ptr -> quat(1); + imu_data.orientation.z = m_sensor_data_ptr -> quat(2); + imu_data.linear_acceleration.x = m_sensor_data_ptr -> a_B(0); + imu_data.linear_acceleration.y = m_sensor_data_ptr -> a_B(1); + imu_data.linear_acceleration.z = m_sensor_data_ptr -> a_B(2); + imu_data.angular_velocity.x = m_sensor_data_ptr -> w_B(0); + imu_data.angular_velocity.y = m_sensor_data_ptr -> w_B(1); + imu_data.angular_velocity.z = m_sensor_data_ptr -> w_B(2); + // Populdate odometry data + + // Populate measurement data from sim + base_position_data.x = m_measurement_data_ptr -> base_position(0); + base_position_data.y = m_measurement_data_ptr -> base_position(1); + base_position_data.z = m_measurement_data_ptr -> base_position(2); + + base_velocity_data.linear.x = m_measurement_data_ptr -> base_velocity.linear(0); + base_velocity_data.linear.y = m_measurement_data_ptr -> base_velocity.linear(1); + base_velocity_data.linear.z = m_measurement_data_ptr -> base_velocity.linear(2); + base_velocity_data.angular.x = m_measurement_data_ptr -> base_velocity.angular(0); + base_velocity_data.angular.y = m_measurement_data_ptr -> base_velocity.angular(1); + base_velocity_data.angular.z = m_measurement_data_ptr -> base_velocity.angular(2); + + base_acceleration_data.linear.x = m_measurement_data_ptr -> base_acceleration.linear(0); + base_acceleration_data.linear.y = m_measurement_data_ptr -> base_acceleration.linear(1); + base_acceleration_data.linear.z = m_measurement_data_ptr -> base_acceleration.linear(2); + base_acceleration_data.angular.x = m_measurement_data_ptr -> base_acceleration.angular(0); + base_acceleration_data.angular.y = m_measurement_data_ptr -> base_acceleration.angular(1); + base_acceleration_data.angular.z = m_measurement_data_ptr -> base_acceleration.angular(2); + + for (int i = 0; i < 12; ++i) { + contact_forces_data[i].data = m_measurement_data_ptr -> contact_force(i); + m_contact_forces_pub[i]->publish(contact_forces_data[i]); + // estimated_fc_data[i].data = m_measurement_data_ptr -> estimated_contact_force(i); + // m_estimated_fc_pub[i]->publish(estimated_fc_data[i]); + } + + // Publish stuff + m_joint_state_pub->publish(js_data); + m_imu_pub->publish(imu_data); + m_odom_pub->publish(odom_data); + m_base_position_pub->publish(base_position_data); + m_base_vel_pub->publish(base_velocity_data); + m_base_acc_pub->publish(base_acceleration_data); + } +} + +void QuadROSComm::Run() { + rclcpp::spin(shared_from_this()); +} \ No newline at end of file diff --git a/locomotion/src/communication/src/SHM.cpp b/locomotion/src/robot_simulation/src/communication/SHM.cpp similarity index 86% rename from locomotion/src/communication/src/SHM.cpp rename to locomotion/src/robot_simulation/src/communication/SHM.cpp index 0d22e96..6848590 100644 --- a/locomotion/src/communication/src/SHM.cpp +++ b/locomotion/src/robot_simulation/src/communication/SHM.cpp @@ -1,6 +1,6 @@ #include "SHM.hpp" -SHM::SHM() : m_name("svan"), CommunicationManager(DATA_ACCESS_MODE::PLANT_TO_EXECUTOR) { +SHM::SHM() : m_name("svan"), CommunicationManager(DATA_ACCESS_MODE::PLANT) { InitClass(); } @@ -56,6 +56,12 @@ void SHM::SetupMemory() { // std::cout << "Sensor data size: " << totalSize << "\n"; sharedMemory = getSHMPointer(key, totalSize); m_sensor_data_ptr = static_cast(sharedMemory); + + key = SHM_KEYS::MEASUREMENT_DATA; + totalSize = sizeof(QuadrupedMeasurementData); + // std::cout << "Sensor data size: " << totalSize << "\n"; + sharedMemory = getSHMPointer(key, totalSize); + m_measurement_data_ptr = static_cast(sharedMemory); // std::cout << "Memory setup completed...\n"; } diff --git a/locomotion/src/robot_simulation/src/main.cpp b/locomotion/src/robot_simulation/src/main.cpp deleted file mode 100644 index 8cb4948..0000000 --- a/locomotion/src/robot_simulation/src/main.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include "mjsim_helper.hpp" -#include "utils.hpp" - -void UpdateSensorData() { - for (int i = 0; i < 12; ++i) { - sensor_data.q(i) = d -> sensordata[i]; - sensor_data.qd(i) = d -> sensordata[12 + i]; - sensor_data.tau(i) = d -> sensordata[24 + i]; - } - for (int i = 0; i < 3; ++i) { - sensor_data.a_B(i) = d -> sensordata[36 + i]; - sensor_data.w_B(i) = d -> sensordata[39 + i]; - } - for (int i = 0; i < 4; ++i) { - sensor_data.quat(i) = d -> sensordata[(42 + ((i + 1) % 4))]; - } - - mat3x3 Rot = pinocchio::quat2rot(sensor_data.quat); - sensor_data.a_W = Rot * sensor_data.a_B; - sensor_data.w_W = Rot * sensor_data.w_B; - - // std::cout << "Quat: " << sensor_data.quat.transpose() << "\n"; - - comm_data.writeSensorData(sensor_data); -} - -void CustomController(const mjModel* model, mjData* data) { - // get joint command - comm_data.getCommandData(joint_command_data); - // controller with sensor readings - if (previous_time == 0) { - previous_time = data->time; - return; - } - // std::cout << "cmd js: " << joint_command_data.q.transpose() << "\n"; - float Kp = 300; - float Kd = 8; - if (data->time - last_update > 1.0/ctrl_update_freq) { - // Get the velocity of joint directly from MuJoCo? - // mjtNum vel = (data->sensordata[0] - position_history)/(d->time-previous_time); - - for (int i = 0; i < 12; ++i) { - float dtheta = joint_command_data.q(i) - sensor_data.q(i); - float dtheta_d = joint_command_data.qd(i) - sensor_data.qd(i); - // data->ctrl[i] = joint_command_data.kp(i) * dtheta * joint_command_data.kd(i) * dtheta_d; - data->ctrl[i] = joint_command_data.tau(i) + Kp * dtheta + Kd * dtheta_d; - } - last_update = data->time; - position_history = data->sensordata[0]; - previous_time = data->time; - } -} - -int main(int argc, char** argv) { - - if (argc != 2) { - std::cerr << "Usage: ./quad_sim [robot_name]\n"; - return 1; - } - - std::string m_name = std::string(argv[1]); - - // std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/" + m_name + ".xml"; - std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/scene.xml"; - - init_model(model_file); - - joint_command_data.q << 0.0, 0.67, -1.3, -0.0, 0.67, -1.3, 0.0, 0.67, -1.3, -0.0, 0.67, -1.3; - - // install control callback - mjcb_control = CustomController; - - timezero = d->time; - double_t update_rate = 0.001; - - // making sure the first time step updates the ctrl previous_time - last_update = timezero-1.0/ctrl_update_freq; - - // comm_data.setSensorDataPtr(&sensor_data); - // comm_data.setCommandDataPtr(&joint_command_data); - - // Run the simulation for 10 seconds with visualization enabled with 60 fps - run_simulation(10000, true, 60); - - shutdown_simulation(); - - return 0; -} \ No newline at end of file diff --git a/locomotion/src/robot_simulation/src/main_ros.cpp b/locomotion/src/robot_simulation/src/sim_main.cpp similarity index 56% rename from locomotion/src/robot_simulation/src/main_ros.cpp rename to locomotion/src/robot_simulation/src/sim_main.cpp index 666cd27..8f4ab98 100644 --- a/locomotion/src/robot_simulation/src/main_ros.cpp +++ b/locomotion/src/robot_simulation/src/sim_main.cpp @@ -1,11 +1,18 @@ -#include "mjsim_helper_ros.hpp" +#include "sim_helper.hpp" #include "utils.hpp" +#include #include +#ifdef USE_ROS_COMM +#include "QuadROSComm.hpp" +#else +#include "SHM.hpp" +#endif + // Signal handler to gracefully handle Ctrl+C void signalHandler(int signum) { - std::cout << "Interrupt signal (" << signum << ") received.\n"; + // std::cout << "Interrupt signal (" << signum << ") received.\n"; // Add cleanup or exit logic as needed exit(signum); } @@ -31,6 +38,22 @@ void UpdateSensorData() { // std::cout << "Quat: " << sensor_data.quat.transpose() << "\n"; comm_data_ptr -> writeSensorData(sensor_data); + + // Updating measurement data + for (int i = 0; i < 3; ++i) { + measurement_data.base_position(i) = d -> sensordata[46 + i]; + measurement_data.base_velocity.linear(i) = d -> sensordata[49 + i]; + measurement_data.base_velocity.angular(i) = d -> sensordata[52 + i]; + measurement_data.base_acceleration.linear(i) = d -> sensordata[55 + i]; + measurement_data.base_acceleration.angular(i) = d -> sensordata[58 + i]; + } + for (int i = 0; i < 4; ++i) { + measurement_data.contact_force(3 * i) = -d -> sensordata[61 + 3 * i]; + measurement_data.contact_force(3 * i + 1) = -d -> sensordata[61 + 3 * i + 1]; + measurement_data.contact_force(3 * i + 2) = -d -> sensordata[61 + 3 * i + 2]; + } + comm_data_ptr -> writeMeasurementData(measurement_data); + } void CustomController(const mjModel* model, mjData* data) { @@ -42,16 +65,17 @@ void CustomController(const mjModel* model, mjData* data) { return; } // std::cout << "cmd js: " << joint_command_data.q.transpose() << "\n"; - float Kp = 300; - float Kd = 8; + // float Kp = 20; + // float Kd = 0.5; + float Kp = 60; + float Kd = 5; + // float Kp = 26.16; + // float Kd = 13.13; if (data->time - last_update > 1.0/ctrl_update_freq) { - // Get the velocity of joint directly from MuJoCo? - // mjtNum vel = (data->sensordata[0] - position_history)/(d->time-previous_time); - for (int i = 0; i < 12; ++i) { float dtheta = joint_command_data.q(i) - sensor_data.q(i); float dtheta_d = joint_command_data.qd(i) - sensor_data.qd(i); - // data->ctrl[i] = joint_command_data.kp(i) * dtheta * joint_command_data.kd(i) * dtheta_d; + // data->ctrl[i] = joint_command_data.tau(i) + joint_command_data.kp(i) * dtheta * joint_command_data.kd(i) * dtheta_d; data->ctrl[i] = joint_command_data.tau(i) + Kp * dtheta + Kd * dtheta_d; } last_update = data->time; @@ -62,7 +86,9 @@ void CustomController(const mjModel* model, mjData* data) { int main(int argc, char** argv) { +#ifdef USE_ROS_COMM rclcpp::init(argc, argv); +#endif if (argc != 2) { std::cerr << "Usage: ./quad_sim [robot_name]\n"; @@ -72,7 +98,10 @@ int main(int argc, char** argv) { std::string m_name = std::string(argv[1]); // std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/" + m_name + ".xml"; - std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/scene.xml"; + // std::string model_file = "/home/meshin/dev/quadruped/xterra/quadruped_locomotion/src/robots/" + m_name + "_description/mujoco/scene.xml"; + std::string rel_model_path = "src/robots/" + m_name + "_description/mujoco/scene.xml"; + std::filesystem::path filepath = std::filesystem::current_path().parent_path().parent_path() / rel_model_path; + std::string model_file = filepath.string(); init_model(model_file); @@ -90,13 +119,23 @@ int main(int argc, char** argv) { // Register signal handler for Ctrl+C signal(SIGINT, signalHandler); - comm_data_ptr = std::make_shared(m_name); - comm_data_ptr->setSensorDataPtr(&sensor_data); +#ifdef USE_ROS_COMM + std::cout << "Using ROS for communication...\n"; + comm_data_ptr = std::make_shared(m_name, DATA_ACCESS_MODE::PLANT); comm_data_ptr->setCommandDataPtr(&joint_command_data); + comm_data_ptr->setSensorDataPtr(&sensor_data); + comm_data_ptr->setMeasurementDataPtr(&measurement_data); +#else + comm_data_ptr = std::make_shared(m_name, DATA_ACCESS_MODE::PLANT); +#endif + // comm_data_ptr -> setAccessMode(DATA_ACCESS_MODE::PLANT); std::thread comm_thread; - comm_thread = std::thread(&CommROSP2E::Run, comm_data_ptr); +#ifdef USE_ROS_COMM + // Run ros on separate non-blocking thread + comm_thread = std::thread(&QuadROSComm::Run, comm_data_ptr); comm_thread.detach(); +#endif // Run the simulation for 10 seconds with visualization enabled with 60 fps run_simulation(10000, true, 60); diff --git a/locomotion/src/robot_simulation/src/test_shm.cpp b/locomotion/src/robot_simulation/src/test_shm.cpp deleted file mode 100644 index 46421b2..0000000 --- a/locomotion/src/robot_simulation/src/test_shm.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "Communication/SHM.hpp" - -int main(int argc, char** argv) { - SHM shm_data(DATA_ACCESS_MODE::EXECUTOR_TO_PLANT); - QuadrupedSensorData sensor_data; - - int i = 0; - while (i < 500000) { - shm_data.getSensorData(sensor_data); - std::cout << "Quat: " << sensor_data.quat.transpose() << "\n"; - i++; - } - - return 0; -} \ No newline at end of file diff --git a/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/ask_for_help.md b/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/ask_for_help.md new file mode 100644 index 0000000..e330354 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/ask_for_help.md @@ -0,0 +1,48 @@ +--- +name: Asking for help +about: Request help from the developers and the community +title: '' +labels: question +assignees: '' + +--- + +**How to ask for help** + +First, read our quick guide to +[asking good questions](https://github.com/google-deepmind/mujoco#asking-questions). +Below is a template for you to use: + +Hi, + +I'm a (student / professor / engineer) and I'm trying to use MuJoCo for _____. + +I'm looking for some help with ____. + +Here is a model which explains my question: + +
+ minimal XML + +```XML + + + + + + + + + + + + + + + + +``` + +
+ +Here is a screenshot / video, illustrating my question: diff --git a/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/bug_report.md b/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000..dff6e4d --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,25 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: bug +assignees: '' + +--- + +**How to submit a good bug report** + +- Are you sure this is a bug? If not, consider using the "Ask for help" or "Feature request" templates. + +- Use a clear and descriptive title. + +- Make it easy to reproduce the problem. + +- Include a ***minimal*** model that demonstrates the problem. If the model is small, include it as inline XML. If it requires binary assets, attach it as a zip file. + +- Include a screenshot or video, if relevant. + +- Include the following context: + - Operating system. + - MuJoCo version (and if the bug is new, the version where it used to work). + - For Python issues, what bindings are you using (i.e `mujoco`, `dm_control`, `mujoco-py`)? \ No newline at end of file diff --git a/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/feature_request.md b/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000..8ca6213 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,20 @@ +--- +name: Feature request +about: Suggest an idea for MuJoCo +title: '' +labels: enhancement +assignees: '' + +--- + +**Is your feature request related to a problem? Please describe.** +A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + +**Describe the solution you'd like** +A clear and concise description of what you want to happen. + +**Describe alternatives you've considered** +A clear and concise description of any alternative solutions or features you've considered. + +**Additional context** +Add any other context or screenshots about the feature request here. \ No newline at end of file diff --git a/locomotion/src/robot_simulation/third_party/mujoco/.gitignore b/locomotion/src/robot_simulation/third_party/mujoco/.gitignore new file mode 100644 index 0000000..c55f0c7 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/.gitignore @@ -0,0 +1,31 @@ +# Don't commit binaries +*.a +*.asm +*.dll +*.dylib +*.exe +*.lib +*.out +*.o +*.so +*.so.* + +# Exclude editor config +.vscode/ +.vs/ + +# Exclude temporary folders +*.egg-info/ +build/ +build_cmake/ + +# Exclude macOS folder attributes +.DS_Store + +# Exclude macOS Info.framework.plist +Info.framework.plist + +# Python byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class diff --git a/locomotion/src/robot_simulation/third_party/mujoco/.readthedocs.yml b/locomotion/src/robot_simulation/third_party/mujoco/.readthedocs.yml new file mode 100644 index 0000000..e8e6f45 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/.readthedocs.yml @@ -0,0 +1,18 @@ +# Read the Docs configuration file +# See https://docs.readthedocs.io/en/stable/config-file/v2.html for details + +version: 2 + +build: + os: ubuntu-20.04 + tools: + python: "3.10" + +sphinx: + builder: html + configuration: doc/conf.py + fail_on_warning: false + +python: + install: + - requirements: doc/requirements.txt diff --git a/locomotion/src/robot_simulation/third_party/mujoco/CMakeLists.txt b/locomotion/src/robot_simulation/third_party/mujoco/CMakeLists.txt new file mode 100644 index 0000000..eea180c --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/CMakeLists.txt @@ -0,0 +1,236 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +cmake_minimum_required(VERSION 3.16) + +# Make CMAKE_C_VISIBILITY_PRESET work properly. +set(CMAKE_POLICY_DEFAULT_CMP0063 NEW) +# INTERPROCEDURAL_OPTIMIZATION is enforced when enabled. +set(CMAKE_POLICY_DEFAULT_CMP0069 NEW) +# Default to GLVND if available. +set(CMAKE_POLICY_DEFAULT_CMP0072 NEW) +# Avoid BUILD_SHARED_LIBS getting overridden by an option() in ccd. +set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) + +# This line has to appear before 'PROJECT' in order to be able to disable incremental linking +set(MSVC_INCREMENTAL_DEFAULT ON) + +project( + mujoco + VERSION 3.1.3 + DESCRIPTION "MuJoCo Physics Simulator" + HOMEPAGE_URL "https://mujoco.org" +) + +enable_language(C) +enable_language(CXX) + +list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") + +option(MUJOCO_BUILD_EXAMPLES "Build samples for MuJoCo" ON) +option(MUJOCO_BUILD_SIMULATE "Build simulate library for MuJoCo" ON) +option(MUJOCO_BUILD_TESTS "Build tests for MuJoCo" ON) +option(MUJOCO_TEST_PYTHON_UTIL "Build and test utility libraries for Python bindings" ON) + +if(APPLE AND (MUJOCO_BUILD_EXAMPLES OR MUJOCO_BUILD_SIMULATE)) + enable_language(OBJC) + enable_language(OBJCXX) +endif() + +include(MujocoOptions) +include(MujocoMacOS) +include(MujocoDependencies) + +set(MUJOCO_HEADERS + include/mujoco/mjdata.h + include/mujoco/mjexport.h + include/mujoco/mjmacro.h + include/mujoco/mjmodel.h + include/mujoco/mjplugin.h + include/mujoco/mjrender.h + include/mujoco/mjthread.h + include/mujoco/mjtnum.h + include/mujoco/mjui.h + include/mujoco/mjvisualize.h + include/mujoco/mjxmacro.h + include/mujoco/mujoco.h +) + +# Add metadata to mujoco.dll when building on Windows. +if(WIN32) + set(MUJOCO_RESOURCE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/dist/mujoco.rc) +else() + set(MUJOCO_RESOURCE_FILES "") +endif() + +add_library(mujoco SHARED ${MUJOCO_RESOURCE_FILES}) +target_include_directories( + mujoco + PUBLIC $ + $ + PRIVATE src +) + +add_subdirectory(plugin/elasticity) +add_subdirectory(plugin/actuator) +add_subdirectory(plugin/sensor) +add_subdirectory(plugin/sdf) +add_subdirectory(src/engine) +add_subdirectory(src/user) +add_subdirectory(src/xml) +add_subdirectory(src/render) +add_subdirectory(src/thread) +add_subdirectory(src/ui) + +target_compile_definitions(mujoco PRIVATE _GNU_SOURCE CCD_STATIC_DEFINE MUJOCO_DLL_EXPORTS -DMC_IMPLEM_ENABLE) +if(MUJOCO_ENABLE_AVX_INTRINSICS) + target_compile_definitions(mujoco PUBLIC mjUSEPLATFORMSIMD) +endif() + +target_compile_options( + mujoco + PRIVATE ${AVX_COMPILE_OPTIONS} + ${MUJOCO_MACOS_COMPILE_OPTIONS} + ${EXTRA_COMPILE_OPTIONS} + ${MUJOCO_CXX_FLAGS} +) +target_link_options( + mujoco + PRIVATE + ${MUJOCO_MACOS_LINK_OPTIONS} + ${EXTRA_LINK_OPTIONS} +) + +target_link_libraries( + mujoco + PRIVATE ccd + lodepng + qhullstatic_r + tinyobjloader + tinyxml2 +) + +set_target_properties( + mujoco PROPERTIES VERSION "${mujoco_VERSION}" PUBLIC_HEADER "${MUJOCO_HEADERS}" +) + +# CMake's built-in FRAMEWORK option doesn't give us control over the dylib name inside the +# Framework. We instead make our own Framework here. +if(APPLE AND MUJOCO_BUILD_MACOS_FRAMEWORKS) + set(TAPI + "/Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/tapi" + ) + configure_file( + ${CMAKE_CURRENT_SOURCE_DIR}/dist/Info.plist.framework.in + ${CMAKE_CURRENT_SOURCE_DIR}/dist/Info.framework.plist + ) + set_target_properties( + mujoco + PROPERTIES LIBRARY_OUTPUT_DIRECTORY + "${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/mujoco.framework/Versions/A" + BUILD_WITH_INSTALL_NAME_DIR TRUE + INSTALL_NAME_DIR "@rpath/mujoco.framework/Versions/A" + ) + add_custom_command( + TARGET mujoco + POST_BUILD + COMMAND mkdir -p $/Headers + COMMAND cd ${CMAKE_CURRENT_SOURCE_DIR} && cp ${MUJOCO_HEADERS} $/Headers + COMMAND mkdir -p $/Modules + COMMAND cp ${CMAKE_CURRENT_SOURCE_DIR}/dist/module.modulemap $/Modules + COMMAND mkdir -p $/Resources + COMMAND mv ${CMAKE_CURRENT_SOURCE_DIR}/dist/Info.framework.plist + $/Resources/Info.plist + COMMAND ${TAPI} stubify $ -o $/mujoco.tbd + COMMAND ln -fhs A $/../Current + COMMAND ln -fhs Versions/Current/mujoco.tbd $/../../mujoco.tbd + COMMAND ln -fhs Versions/Current/Headers $/../../Headers + COMMAND ln -fhs Versions/Current/Modules $/../../Modules + COMMAND ln -fhs Versions/Current/Resources $/../../Resources + COMMAND_EXPAND_LISTS + ) +endif() + +# Add a namespace alias to mujoco to be used by the examples. +# This simulates the install interface when building with sources. +add_library(mujoco::mujoco ALIAS mujoco) + +add_subdirectory(model) + +# `simulate` defines a macro, embed_in_bundle, that's used by `sample`, so that +# subdirectory needs to be added first. +# TODO: Remove this order dependency. +if(MUJOCO_BUILD_SIMULATE) + add_subdirectory(simulate) +endif() + +if(MUJOCO_BUILD_EXAMPLES) + add_subdirectory(sample) +endif() + +if(BUILD_TESTING AND MUJOCO_BUILD_TESTS) + enable_testing() + add_subdirectory(test) + if(MUJOCO_TEST_PYTHON_UTIL) + add_subdirectory(python/mujoco/util) + endif() +endif() + +if(NOT (APPLE AND MUJOCO_BUILD_MACOS_FRAMEWORKS)) + # Install the libraries. + install( + TARGETS mujoco + EXPORT ${PROJECT_NAME} + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime + LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT runtime + ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT dev + PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/mujoco" COMPONENT dev + ) + + set(CONFIG_PACKAGE_LOCATION "${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME}") + + # Generate and install the mujocoTargets.cmake file. This defines the targets as + # IMPORTED libraries for downstream users. + install( + EXPORT ${PROJECT_NAME} + DESTINATION ${CONFIG_PACKAGE_LOCATION} + NAMESPACE mujoco:: + FILE "${PROJECT_NAME}Targets.cmake" + ) + + include(CMakePackageConfigHelpers) + + write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + VERSION ${mujoco_VERSION} + COMPATIBILITY AnyNewerVersion + ) + + configure_package_config_file( + cmake/${PROJECT_NAME}Config.cmake.in "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION ${CONFIG_PACKAGE_LOCATION} + ) + + install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION ${CONFIG_PACKAGE_LOCATION} + ) + + # Install also models into share folder. + install( + DIRECTORY model + DESTINATION "${CMAKE_INSTALL_DATADIR}/mujoco" + PATTERN "CMakeLists.txt" EXCLUDE + ) +endif() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/CONTRIBUTING.md b/locomotion/src/robot_simulation/third_party/mujoco/CONTRIBUTING.md new file mode 100644 index 0000000..2c4672e --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/CONTRIBUTING.md @@ -0,0 +1,116 @@ +# Contributing to MuJoCo + +We intend for MuJoCo to be a true community-driven project and look forward to +accepting your contributions! + +## Before you contribute + +### Documentation, forums + +Please read MuJoCo's [documentation](https://mujoco.readthedocs.io/) and look +through current topics on our GitHub +[issues](https://github.com/google-deepmind/mujoco/issues) and +[discussions](https://github.com/google-deepmind/mujoco/discussions) pages. + +### Contributor License Agreement + +Contributions to this project must be accompanied by a Contributor License +Agreement (CLA). You (or your employer) retain the copyright to your +contribution; this simply gives us permission to use and redistribute your +contributions as part of the project. Head over to + to see your current agreements on file or +to sign a new one. + +You generally only need to submit a CLA once, so if you've already submitted one +(even if it was for a different project), you probably don't need to do it +again. + +## Contributing + +### Reporting bugs + +How to submit a good bug report: + +- Use a clear and descriptive title. + +- Make it easy to reproduce the problem. If this requires a model, attach it as +a zip file to the bug report. The model and steps required to reproduce the +problem should be *minimal*, in the sense that irrelevant parts are +removed. + +- Clearly state what is the expected behavior. + +- Include an illustrative screenshot, if relevant. + +Try to provide context: + +- If the problem is new, see if you can reproduce it in an older version. +What's the most recent version in which the problem doesn't happen? + +- Can you reproduce the problem on multiple platforms? + +### Suggesting enhancements + +Before submitting an enhancement suggestion: + +- Check if you're using the [latest +version](https://github.com/google-deepmind/mujoco/releases/latest) of MuJoCo. + +- Perform a quick [search](https://github.com/google-deepmind/mujoco/issues) to +see if the enhancement has already been suggested. If it has, add a comment to +the existing issue instead of opening a new one. + +How to submit a good enhancement suggestion: + +- Use a clear and descriptive title. + +- Describe the current behaviour and the behavior which you hope to see instead. + +- Explain why this enhancement would be useful. + +- Specify the version of MuJoCo and platform/OS you are using. + +### Contributing code + +- Except for small and straightforward bugfixes, please get in touch with us +before you start working on a contribution so that we can help and possibly +guide you. Coordinating up front makes it much easier to avoid frustration later +on. + +- All submissions require review. Please use GitHub pull requests for this +purpose. Please consult +[GitHub Help](https://help.github.com/articles/about-pull-requests/) for more +information on pull requests. + +- Write tests. MuJoCo uses [googletest](https://github.com/google/googletest) +for C++ tests, [absltest](https://abseil.io/docs/python/guides/testing) for +Python binding code and [nunit](https://nunit.org/) for C# code in the Unity +plugin. In most cases, a pull request will only be accepted if it includes +tests. MuJoCo's internal codebase is currently lacking in test coverage. If you +want to modify a function that isn't covered by tests, you'll be expected to +contribute tests for the existing functionality, not just your modification. In +fact, writing a test for existing code is a great way to get started with +contributions. + +- Resolve compiler warnings. + +- All existing tests must pass. + +- Follow the [Style Guide](./STYLEGUIDE.md). In particular, adequately comment +your code. + +- Make small pull requests. We will likely ask you to split up a large pull +request into self-contained, smaller ones, especially if the PR is trying to +achieve multiple things. + +- Respond to reviewers. Please be responsive to any questions and comments. + +Once you have met all the requirements, your code will be merged. +Thanks for improving MuJoCo! + + + +### Community guidelines + +This project follows Google's +[Open Source Community Guidelines](https://opensource.google/conduct/). diff --git a/locomotion/src/robot_simulation/third_party/mujoco/LICENSE b/locomotion/src/robot_simulation/third_party/mujoco/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/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. diff --git a/locomotion/src/robot_simulation/third_party/mujoco/README.md b/locomotion/src/robot_simulation/third_party/mujoco/README.md new file mode 100644 index 0000000..cb19a63 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/README.md @@ -0,0 +1,220 @@ +

+ MuJoCo +

+ +

+ + + + + + + + + +

+ +**MuJoCo** stands for **Mu**lti-**Jo**int dynamics with **Co**ntact. It is a +general purpose physics engine that aims to facilitate research and development +in robotics, biomechanics, graphics and animation, machine learning, and other +areas which demand fast and accurate simulation of articulated structures +interacting with their environment. + +This repository is maintained by [Google DeepMind](https://www.deepmind.com/). + +MuJoCo has a C API and is intended for researchers and developers. The runtime +simulation module is tuned to maximize performance and operates on low-level +data structures that are preallocated by the built-in XML compiler. The library +includes interactive visualization with a native GUI, rendered in OpenGL. MuJoCo +further exposes a large number of utility functions for computing +physics-related quantities. + +We also provide [Python bindings] and a plug-in for the [Unity] game engine. + +## Documentation + +MuJoCo's documentation can be found at [mujoco.readthedocs.io]. Upcoming features due for the next +release can be found in the [changelog] in the latest branch. + +## Getting Started + +There are two easy ways to get started with MuJoCo: + +1. **Run `simulate` on your machine.** +[This video](https://www.youtube.com/watch?v=0ORsj_E17B0) shows a screen capture +of `simulate`, MuJoCo's native interactive viewer. Follow the steps described in +the [Getting Started] section of the documentation to get `simulate` running on +your machine. + +2. **Explore our online IPython notebooks.** +If you are a Python user, you might want to start with our tutorial notebooks +running on Google Colab: + + - The first tutorial focuses on the basics of MuJoCo: + [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/tutorial.ipynb) + - For a more advanced example, see the LQR tutorial which creates an LQR + controller to balance a humanoid on one leg using MuJoCo's dynamics + derivatives: [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/LQR.ipynb) + - The MJX tutorial provides usage examples of + [MuJoCo XLA](https://mujoco.readthedocs.io/en/stable/mjx.html), a branch of MuJoCo written in + JAX: + [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/mjx/tutorial.ipynb) + +## Installation + +### Prebuilt binaries + +Versioned releases are available as precompiled binaries from the GitHub +[releases page], built for Linux (x86-64 and AArch64), Windows (x86-64 only), +and macOS (universal). This is the recommended way to use the software. + +### Building from source + +Users who wish to build MuJoCo from source should consult the [build from +source] section of the documentation. However, please note that the commit at +the tip of the `main` branch may be unstable. + +### Python (>= 3.8) + +The native Python bindings, which come pre-packaged with a copy of MuJoCo, can +be installed from [PyPI] via: + +```bash +pip install mujoco +``` + +Note that Pre-built Linux wheels target `manylinux2014`, see +[here](https://github.com/pypa/manylinux) for compatible distributions. For more +information such as building the bindings from source, see the [Python bindings] +section of the documentation. + +## Contributing + +We welcome community engagement: questions, requests for help, bug reports and +feature requests. To read more about bug reports, feature requests and more +ambitious contributions, please see our [contributors guide](CONTRIBUTING.md) +and [style guide](STYLEGUIDE.md). + +## Asking Questions + +Questions and requests for help are welcome on the GitHub +[Issues](https://github.com/google-deepmind/mujoco/issues) page and should focus +on a specific problem or question. + +[Discussions](https://github.com/google-deepmind/mujoco/discussions) should +address wider concerns that might require input from multiple participants. + +Here are some guidelines for asking good questions: + +1. Search for existing questions or issues that touch on the same subject. + + You can add comments to existing threads or start new ones. If you start a + new thread and there are existing relevant threads, please link to them. + +2. Use a clear and specific title. Try to include keywords that will make your + question easy for other to find in the future. + +3. Introduce yourself and your project more generally. + + If your level of expertise is exceptional (either high or low), and it might + be relevant to what we can assume you know, please state that as well. + +4. Take a step back and tell us what you're trying to accomplish, if we + understand you goal we might suggest a different type of solution than the + one you are having problems with + +5. Make it easy for others to reproduce the problem or understand your question. + + If this requires a model, please include it. Try to make the model minimal: + remove elements that are unrelated to your question. Pure XML models should + be inlined. Models requiring binary assets (meshes, textures), should be + attached as a `.zip` file. Please make sure the included model is loadable + before you attach it. + +6. Include an illustrative screenshot or video, if relevant. + +7. Tell us how you are accessing MuJoCo (C API, Python bindings, etc.) and which + MuJoCo version and operating system you are using. + +## Related software +MuJoCo forms the backbone of many environment packages, but these are too many +to list here individually. Below we focus on bindings and converters. + +### Bindings + +These packages give users of various languages access to MuJoCo functionality: + +#### First-party bindings: + +- [Python bindings](https://mujoco.readthedocs.io/en/stable/python.html) + - [dm_control](https://github.com/google-deepmind/dm_control), Google + DeepMind's related environment stack, includes + [PyMJCF](https://github.com/google-deepmind/dm_control/blob/main/dm_control/mjcf/README.md), + a module for procedural manipulation of MuJoCo models. +- [C# bindings and Unity plug-in](https://mujoco.readthedocs.io/en/stable/unity.html) + +#### Third-party bindings: + +- **WebAssembly**: [mujoco_wasm](https://github.com/zalo/mujoco_wasm) by [@zalo](https://github.com/zalo) with contributions by + [@kevinzakka](https://github.com/kevinzakka), based on the [emscripten build](https://github.com/stillonearth/MuJoCo-WASM) by + [@stillonearth](https://github.com/stillonearth). + + :arrow_right: [Click here](https://zalo.github.io/mujoco_wasm/) for a live demo of MuJoCo running in your browser. +- **MATLAB Simulink**: [Simulink Blockset for MuJoCo Simulator](https://github.com/mathworks-robotics/mujoco-simulink-blockset) + by [Manoj Velmurugan](https://github.com/vmanoj1996). +- **Swift**: [swift-mujoco](https://github.com/liuliu/swift-mujoco) +- **Java**: [mujoco-java](https://github.com/CommonWealthRobotics/mujoco-java) +- **Julia**: [MuJoCo.jl](https://github.com/JamieMair/MuJoCo.jl) + + +### Converters + +- **OpenSim**: [MyoConverter](https://github.com/MyoHub/myoconverter) converts + OpenSim models to MJCF. +- **SDFormat**: [gz-mujoco](https://github.com/gazebosim/gz-mujoco/) is a + two-way SDFormat <-> MJCF conversion tool. +- **OBJ**: [obj2mjcf](https://github.com/kevinzakka/obj2mjcf) + a script for converting composite OBJ files into a loadable MJCF model. + +## Citation + +If you use MuJoCo for published research, please cite: + +``` +@inproceedings{todorov2012mujoco, + title={MuJoCo: A physics engine for model-based control}, + author={Todorov, Emanuel and Erez, Tom and Tassa, Yuval}, + booktitle={2012 IEEE/RSJ International Conference on Intelligent Robots and Systems}, + pages={5026--5033}, + year={2012}, + organization={IEEE}, + doi={10.1109/IROS.2012.6386109} +} +``` + +## License and Disclaimer + +Copyright 2021 DeepMind Technologies Limited. + +Box collision code ([`engine_collision_box.c`](https://github.com/google-deepmind/mujoco/blob/main/src/engine/engine_collision_box.c)) +is Copyright 2016 Svetoslav Kolev. + +ReStructuredText documents, images, and videos in the `doc` directory are made +available under the terms of the Creative Commons Attribution 4.0 (CC BY 4.0) +license. You may obtain a copy of the License at +https://creativecommons.org/licenses/by/4.0/legalcode. + +Source code is licensed under the Apache License, Version 2.0. You may obtain a +copy of the License at https://www.apache.org/licenses/LICENSE-2.0. + +This is not an officially supported Google product. + +[build from source]: https://mujoco.readthedocs.io/en/latest/programming#building-mujoco-from-source +[Getting Started]: https://mujoco.readthedocs.io/en/latest/programming#getting-started +[Unity]: https://unity.com/ +[releases page]: https://github.com/google-deepmind/mujoco/releases +[GitHub Issues]: https://github.com/google-deepmind/mujoco/issues +[mujoco.readthedocs.io]: https://mujoco.readthedocs.io +[changelog]: https://mujoco.readthedocs.io/en/latest/changelog.html +[Python bindings]: https://mujoco.readthedocs.io/en/stable/python.html#python-bindings +[PyPI]: https://pypi.org/project/mujoco/ diff --git a/locomotion/src/robot_simulation/third_party/mujoco/SECURITY.md b/locomotion/src/robot_simulation/third_party/mujoco/SECURITY.md new file mode 100644 index 0000000..4648e5e --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/SECURITY.md @@ -0,0 +1,4 @@ +To report a security issue, please use [https://g.co/vulnz](https://g.co/vulnz). +We use g.co/vulnz for our intake, and do coordination and disclosure here on +GitHub (including using GitHub Security Advisory). The Google Security Team will +respond within 5 working days of your report on g.co/vulnz. diff --git a/locomotion/src/robot_simulation/third_party/mujoco/STYLEGUIDE.md b/locomotion/src/robot_simulation/third_party/mujoco/STYLEGUIDE.md new file mode 100644 index 0000000..5c97f0b --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/STYLEGUIDE.md @@ -0,0 +1,160 @@ +# MuJoCo Style Guide + +The MuJoCo codebase follows an internally consistent style that values +compactness and readability. Please try to follow the style guide as closely as +possible in your code contributions. + +### Scope of this guide + +MuJoCo has three main code categories: + +1. **C code:** MuJoCo's core codebase. It consists of public headers under +`include/` and C source files and internal headers under `src/`. This style +guide primarily concerns itself with this category. + +2. **Legacy C++:** Files under `src/user/` and `src/xml/`. These do not +necessarily follow best C++ practices. We intend to gradually replace these with +new code that follows the [Google C++ +style](https://google.github.io/styleguide/cppguide.html) over time. + +3. **New code:** This includes C++ files under `test/` and `python/` and C# +files under `unity/`. Added by DeepMind engineers, this code adheres to the +[Google style](https://google.github.io/styleguide/). + +### General principles + +Where any aspect of coding style is not explicitly spelled out in this guide, +the following principle is followed: + +| Maximise consistency with the rest of the code. | +| --- | + +If there is a contradiction between this guide and existing code, the guide +takes precedence. Additional principles include: + +- Follow the [naming conventions](https://mujoco.readthedocs.io/en/latest/programming#naming-convention). +- Be sparing with horizontal space: Try to keep lines short, avoid line-breaks + where possible. +- Be generous with vertical space: Empty lines between code blocks are good. +- Keep names short. +- Inline comments are part of the code, treat them as such. +- Use American English in comments and documentation. + +### Specific rules for C code + +Over time, this style guide will be expanded to cover most aspects of C +programming in the MuJoCo codebase. In the meantime, it is usually enough to +inspect existing code and try to follow its example. + +If there are any consistent coding patterns that are specific to the MuJoCo +codebase but aren't mentioned in the guide, the guide should be expanded. If you +spot such a pattern, feel free to send a PR to update the guide. + +#### Indentation + +2-space indents, using space characters rather than tabs. + +#### Line length + +Line length is 100 characters. In rare situations, like the collision table at +the top of +[engine_collision_driver.c](https://github.com/google-deepmind/mujoco/blob/c8ff7b3d341560e8cc33fbdcaffbcdbc4c32327c/src/engine/engine_collision_driver.c#L36), +longer lines are allowed for readability. + +#### Comments + +MuJoCo makes generous use of short, one-line comments describing the code block +just below them. They are considered an essential part of the code. Comments +should be: + +- As succinct as possible, while maintaining clarity. +- Preceded by an empty line, unless at the top of a block. +- Uncapitalized and not terminated by a full-stop. + +A helpful heuristic regarding in-code comments is that the reader should be able +to get a sense of what is happening in a function just by reading the comments. + +An exception to the third bullet point above are function declaration comments +in public header files which are considered to be docstrings rather than code +and are therefore capitalized and terminated by a full stop. These docstrings +are required. + +#### Braces + +- MuJoCo uses +[attached K&R braces](https://en.wikipedia.org/wiki/Indentation_style#Variant:_mandatory_braces), +including for one-line blocks: + + ```C + // transpose matrix + void mju_transpose(mjtNum* res, const mjtNum* mat, int nr, int nc) { + for (int i=0; i < nr; i++) { + for (int j=0; j < nc; j++) { + res[j*nr+i] = mat[i*nc+j]; + } + } + } + ``` + +- Brace-less single line statements are allowed outside of `engine/` code, for +similar, repeated blocks, that do not contain flow control statements (`return`, +`continue`, etc.). For an example of this exception, inspect the [`mjCModel` +destructor](https://github.com/google-deepmind/mujoco/search?q=repo%3Adeepmind%2Fmujoco+filename%3Auser_model.cc). + +- Unattached braces are allowed in `if/else` blocks, when inserting a comment +before the `else`: + + ```C + // rotate vector by quaternion + void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) { + // null quat: copy vec + if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) { + mju_copy3(res, vec); + } + + // regular processing + else { + mjtNum mat[9]; + mju_quat2Mat(mat, quat); + mju_rotVecMat(res, vec, mat); + } + } + ``` + +#### Spacing + +- MuJoCo encourages judicious use of spacing around operators to promote +readability. For example below, note the lack of spaces around the +multiplication operator, and the aligning spaces in the second and fourth +assignments: + + ```C + // time-derivative of quaternion, given 3D rotational velocity + void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]) { + res[0] = 0.5*(-vel[0]*quat[1] - vel[1]*quat[2] - vel[2]*quat[3]); + res[1] = 0.5*( vel[0]*quat[0] + vel[1]*quat[3] - vel[2]*quat[2]); + res[2] = 0.5*(-vel[0]*quat[3] + vel[1]*quat[0] + vel[2]*quat[1]); + res[3] = 0.5*( vel[0]*quat[2] - vel[1]*quat[1] + vel[2]*quat[0]); + } + ``` + +- Spaces are required around comparison operators. + +- Spaces are not allowed around operators in array subscripts `[]` or in + variable initialisation in `for` loops. For example, inspect the + `mju_transpose` implementation above. + +- Three blank lines are required between function implementations in source files. + +#### Variable declarations + +Historically the MuJoCo C codebase used exclusively C89-style variable +declarations, with all stack variables pre-declared at the top of the function. +We are in the process of migrating the code to the C99 convention of declaring +variables at the narrowest possible scope. For example iterator variables in +for-loops are mostly declared in the narrow scope, as in the `mju_transpose` +example above. + +New code should use the C99 convention. When editing an existing function, +please move existing variable declarations into local scope. Pull requests +helping us to complete the migration are very welcome. diff --git a/locomotion/src/robot_simulation/third_party/mujoco/banner.png b/locomotion/src/robot_simulation/third_party/mujoco/banner.png new file mode 100644 index 0000000..a655156 Binary files /dev/null and b/locomotion/src/robot_simulation/third_party/mujoco/banner.png differ diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/CheckAvxSupport.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/CheckAvxSupport.cmake new file mode 100644 index 0000000..52e8b23 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/CheckAvxSupport.cmake @@ -0,0 +1,56 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +include(CheckCSourceCompiles) + +# Assigns compiler options to the given variable based on availability of AVX. +function(get_avx_compile_options OUTPUT_VAR) + message(VERBOSE "Checking if AVX is available...") + + if(MSVC) + set(CMAKE_REQUIRED_FLAGS "/arch:AVX") + elseif(WIN32) + # Abseil LTS 20230125.0 assumes that AVX implies PCLMUL on Windows. + set(CMAKE_REQUIRED_FLAGS "-mavx" "-mpclmul") + else() + set(CMAKE_REQUIRED_FLAGS "-mavx") + endif() + + if(APPLE AND "x86_64" IN_LIST CMAKE_OSX_ARCHITECTURES) + message(STATUS "Building x86_64 on macOS, forcing CAN_BUILD_AVX to TRUE.") + set(CAN_BUILD_AVX TRUE) + else() + check_c_source_compiles( + " + #include + int main(int argc, char* argv[]) { + __m256d ymm; + return 0; + } + " + CAN_BUILD_AVX + ) + endif() + + if(CAN_BUILD_AVX) + message(VERBOSE "Checking if AVX is available... AVX available.") + set("${OUTPUT_VAR}" + ${CMAKE_REQUIRED_FLAGS} + PARENT_SCOPE + ) + else() + message(VERBOSE "Checking if AVX is available... AVX not available.") + set("${OUTPUT_VAR}" PARENT_SCOPE) + endif() +endfunction() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/FindOrFetch.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/FindOrFetch.cmake new file mode 100644 index 0000000..f311b2c --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/FindOrFetch.cmake @@ -0,0 +1,140 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# 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 +# +# https://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. +# +#.rst: +# FindOrFetch +# ---------------------- +# +# Find or fetch a package in order to satisfy target dependencies. +# +# FindOrFetch([USE_SYSTEM_PACKAGE [ON/OFF]] +# [PACKAGE_NAME [name]] +# [LIBRARY_NAME [name]] +# [GIT_REPO [repo]] +# [GIT_TAG [tag]] +# [PATCH_COMMAND [cmd] [args]] +# [TARGETS [targets]] +# [EXCLUDE_FROM_ALL]) +# +# The command has the following parameters: +# +# Arguments: +# - ``USE_SYSTEM_PACKAGE`` one-value argument on whether to search for the +# package in the system (ON) or whether to fetch the library using +# FetchContent from the specified Git repository (OFF). Note that +# FetchContent variables will override this behaviour. +# - ``PACKAGE_NAME`` name of the system-package. Ignored if +# ``USE_SYSTEM_PACKAGE`` is ``OFF``. +# - ``LIBRARY_NAME`` name of the library. Ignored if +# ``USE_SYSTEM_PACKAGE`` is ``ON``. +# - ``GIT_REPO`` git repository to fetch the library from. Ignored if +# ``USE_SYSTEM_PACKAGE`` is ``ON``. +# - ``GIT_TAG`` tag reference when fetching the library from the git +# repository. Ignored if ``USE_SYSTEM_PACKAGE`` is ``ON``. +# - ``PATCH_COMMAND`` Specifies a custom command to patch the sources after an +# update. See https://cmake.org/cmake/help/latest/module/ExternalProject.html#command:externalproject_add +# for details on the parameter. +# - ``TARGETS`` list of targets to be satisfied. If any of these targets are +# not currently defined, this macro will attempt to either find or fetch the +# package. +# - ``EXCLUDE_FROM_ALL`` if specified, the targets are not added to the ``all`` +# metatarget. +# +# Note: if ``USE_SYSTEM_PACKAGE`` is ``OFF``, FetchContent will be used to +# retrieve the specified targets. It is possible to specify any variable in +# https://cmake.org/cmake/help/latest/module/FetchContent.html#variables to +# override this macro behaviour. + +if(COMMAND FindOrFetch) + return() +endif() + +macro(FindOrFetch) + if(NOT FetchContent) + include(FetchContent) + endif() + + # Parse arguments. + set(options EXCLUDE_FROM_ALL) + set(one_value_args + USE_SYSTEM_PACKAGE + PACKAGE_NAME + LIBRARY_NAME + GIT_REPO + GIT_TAG + ) + set(multi_value_args PATCH_COMMAND TARGETS) + cmake_parse_arguments( + _ARGS + "${options}" + "${one_value_args}" + "${multi_value_args}" + ${ARGN} + ) + + # Check if all targets are found. + if(NOT _ARGS_TARGETS) + message(FATAL_ERROR "mujoco::FindOrFetch: TARGETS must be specified.") + endif() + + set(targets_found TRUE) + message(CHECK_START + "mujoco::FindOrFetch: checking for targets in package `${_ARGS_PACKAGE_NAME}`" + ) + foreach(target ${_ARGS_TARGETS}) + if(NOT TARGET ${target}) + message(CHECK_FAIL "target `${target}` not defined.") + set(targets_found FALSE) + break() + endif() + endforeach() + + # If targets are not found, use `find_package` or `FetchContent...` to get it. + if(NOT targets_found) + if(${_ARGS_USE_SYSTEM_PACKAGE}) + message(CHECK_START + "mujoco::FindOrFetch: finding `${_ARGS_PACKAGE_NAME}` in system packages..." + ) + find_package(${_ARGS_PACKAGE_NAME} REQUIRED) + message(CHECK_PASS "found") + else() + message(CHECK_START + "mujoco::FindOrFetch: Using FetchContent to retrieve `${_ARGS_LIBRARY_NAME}`" + ) + FetchContent_Declare( + ${_ARGS_LIBRARY_NAME} + GIT_REPOSITORY ${_ARGS_GIT_REPO} + GIT_TAG ${_ARGS_GIT_TAG} + GIT_SHALLOW FALSE + PATCH_COMMAND ${_ARGS_PATCH_COMMAND} + UPDATE_DISCONNECTED TRUE + ) + if(${_ARGS_EXCLUDE_FROM_ALL}) + FetchContent_GetProperties(${_ARGS_LIBRARY_NAME}) + if(NOT ${${_ARGS_LIBRARY_NAME}_POPULATED}) + FetchContent_Populate(${_ARGS_LIBRARY_NAME}) + add_subdirectory( + ${${_ARGS_LIBRARY_NAME}_SOURCE_DIR} ${${_ARGS_LIBRARY_NAME}_BINARY_DIR} + EXCLUDE_FROM_ALL + ) + endif() + else() + FetchContent_MakeAvailable(${_ARGS_LIBRARY_NAME}) + endif() + message(CHECK_PASS "Done") + endif() + else() + message(CHECK_PASS "found") + endif() +endmacro() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoDependencies.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoDependencies.cmake new file mode 100644 index 0000000..92341a6 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoDependencies.cmake @@ -0,0 +1,359 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +# Build configuration for third party libraries used in MuJoCo. + +set(MUJOCO_DEP_VERSION_lodepng + b4ed2cd7ecf61d29076169b49199371456d4f90b + CACHE STRING "Version of `lodepng` to be fetched." +) +set(MUJOCO_DEP_VERSION_tinyxml2 + 9a89766acc42ddfa9e7133c7d81a5bda108a0ade + CACHE STRING "Version of `tinyxml2` to be fetched." +) +set(MUJOCO_DEP_VERSION_tinyobjloader + 1421a10d6ed9742f5b2c1766d22faa6cfbc56248 + CACHE STRING "Version of `tinyobjloader` to be fetched." +) +set(MUJOCO_DEP_VERSION_MarchingCubeCpp + 5b79e5d6bded086a0abe276a4b5a69fc17ae9bf1 + CACHE STRING "Version of `MarchingCubeCpp` to be fetched." +) +set(MUJOCO_DEP_VERSION_ccd + 7931e764a19ef6b21b443376c699bbc9c6d4fba8 # v2.1 + CACHE STRING "Version of `ccd` to be fetched." +) +set(MUJOCO_DEP_VERSION_qhull + 0c8fc90d2037588024d9964515c1e684f6007ecc + CACHE STRING "Version of `qhull` to be fetched." +) +set(MUJOCO_DEP_VERSION_Eigen3 + 7fd7a3f946e5ac152d28dad388cff8bfa1026925 + CACHE STRING "Version of `Eigen3` to be fetched." +) + +set(MUJOCO_DEP_VERSION_abseil + 4a2c63365eff8823a5221db86ef490e828306f9d # LTS 20240116.0 + CACHE STRING "Version of `abseil` to be fetched." +) + +set(MUJOCO_DEP_VERSION_gtest + f8d7d77c06936315286eb55f8de22cd23c188571 # v1.14.0 + CACHE STRING "Version of `gtest` to be fetched." +) + +set(MUJOCO_DEP_VERSION_benchmark + e45585a4b8e75c28479fa4107182c28172799640 # v1.8.3 + CACHE STRING "Version of `benchmark` to be fetched." +) + +set(MUJOCO_DEP_VERSION_sdflib + 7c49cfba9bbec763b5d0f7b90b26555f3dde8088 + CACHE STRING "Version of `SdfLib` to be fetched." +) + +mark_as_advanced(MUJOCO_DEP_VERSION_lodepng) +mark_as_advanced(MUJOCO_DEP_VERSION_MarchingCubeCpp) +mark_as_advanced(MUJOCO_DEP_VERSION_tinyxml2) +mark_as_advanced(MUJOCO_DEP_VERSION_tinyobjloader) +mark_as_advanced(MUJOCO_DEP_VERSION_ccd) +mark_as_advanced(MUJOCO_DEP_VERSION_qhull) +mark_as_advanced(MUJOCO_DEP_VERSION_Eigen3) +mark_as_advanced(MUJOCO_DEP_VERSION_abseil) +mark_as_advanced(MUJOCO_DEP_VERSION_gtest) +mark_as_advanced(MUJOCO_DEP_VERSION_benchmark) +mark_as_advanced(MUJOCO_DEP_VERSION_sdflib) + +include(FetchContent) +include(FindOrFetch) + +# Override the BUILD_SHARED_LIBS setting, just for building third party libs (since we always want +# static libraries). The ccd CMakeLists.txt doesn't expose an option to build a static ccd library, +# unless BUILD_SHARED_LIBS is set. + +# We force all the dependencies to be compiled as static libraries. +# TODO(fraromano) Revisit this choice when adding support for install. +set(BUILD_SHARED_LIBS_OLD ${BUILD_SHARED_LIBS}) +set(BUILD_SHARED_LIBS + OFF + CACHE INTERNAL "Build SHARED libraries" +) + +if(NOT TARGET lodepng) + FetchContent_Declare( + lodepng + GIT_REPOSITORY https://github.com/lvandeve/lodepng.git + GIT_TAG ${MUJOCO_DEP_VERSION_lodepng} + ) + + FetchContent_GetProperties(lodepng) + if(NOT lodepng_POPULATED) + FetchContent_Populate(lodepng) + # This is not a CMake project. + set(LODEPNG_SRCS ${lodepng_SOURCE_DIR}/lodepng.cpp) + set(LODEPNG_HEADERS ${lodepng_SOURCE_DIR}/lodepng.h) + add_library(lodepng STATIC ${LODEPNG_HEADERS} ${LODEPNG_SRCS}) + target_compile_options(lodepng PRIVATE ${MUJOCO_MACOS_COMPILE_OPTIONS}) + target_link_options(lodepng PRIVATE ${MUJOCO_MACOS_LINK_OPTIONS}) + target_include_directories(lodepng PUBLIC ${lodepng_SOURCE_DIR}) + endif() +endif() + +if(NOT TARGET marchingcubecpp) + FetchContent_Declare( + marchingcubecpp + GIT_REPOSITORY https://github.com/aparis69/MarchingCubeCpp.git + GIT_TAG ${MUJOCO_DEP_VERSION_MarchingCubeCpp} + ) + + FetchContent_GetProperties(marchingcubecpp) + if(NOT marchingcubecpp_POPULATED) + FetchContent_Populate(marchingcubecpp) + include_directories(${marchingcubecpp_SOURCE_DIR}) + endif() +endif() + +set(QHULL_ENABLE_TESTING OFF) + +findorfetch( + USE_SYSTEM_PACKAGE + OFF + PACKAGE_NAME + qhull + LIBRARY_NAME + qhull + GIT_REPO + https://github.com/qhull/qhull.git + GIT_TAG + ${MUJOCO_DEP_VERSION_qhull} + TARGETS + qhull + EXCLUDE_FROM_ALL +) +# MuJoCo includes a file from libqhull_r which is not exported by the qhull include directories. +# Add it to the target. +target_include_directories( + qhullstatic_r INTERFACE $ +) +target_compile_options(qhullstatic_r PRIVATE ${MUJOCO_MACOS_COMPILE_OPTIONS}) +target_link_options(qhullstatic_r PRIVATE ${MUJOCO_MACOS_LINK_OPTIONS}) + +set(tinyxml2_BUILD_TESTING OFF) +findorfetch( + USE_SYSTEM_PACKAGE + OFF + PACKAGE_NAME + tinyxml2 + LIBRARY_NAME + tinyxml2 + GIT_REPO + https://github.com/leethomason/tinyxml2.git + GIT_TAG + ${MUJOCO_DEP_VERSION_tinyxml2} + TARGETS + tinyxml2 + EXCLUDE_FROM_ALL +) +target_compile_options(tinyxml2 PRIVATE ${MUJOCO_MACOS_COMPILE_OPTIONS}) +target_link_options(tinyxml2 PRIVATE ${MUJOCO_MACOS_LINK_OPTIONS}) + +findorfetch( + USE_SYSTEM_PACKAGE + OFF + PACKAGE_NAME + tinyobjloader + LIBRARY_NAME + tinyobjloader + GIT_REPO + https://github.com/tinyobjloader/tinyobjloader.git + GIT_TAG + ${MUJOCO_DEP_VERSION_tinyobjloader} + TARGETS + tinyobjloader + EXCLUDE_FROM_ALL +) + +option(SDFLIB_USE_ASSIMP OFF) +option(SDFLIB_USE_OPENMP OFF) +option(SDFLIB_USE_ENOKI OFF) +findorfetch( + USE_SYSTEM_PACKAGE + OFF + PACKAGE_NAME + sdflib + LIBRARY_NAME + sdflib + GIT_REPO + https://github.com/UPC-ViRVIG/SdfLib.git + GIT_TAG + ${MUJOCO_DEP_VERSION_sdflib} + TARGETS + SdfLib + EXCLUDE_FROM_ALL +) +target_compile_options(SdfLib PRIVATE ${MUJOCO_MACOS_COMPILE_OPTIONS}) +target_link_options(SdfLib PRIVATE ${MUJOCO_MACOS_LINK_OPTIONS}) + +set(ENABLE_DOUBLE_PRECISION ON) +set(CCD_HIDE_ALL_SYMBOLS ON) +findorfetch( + USE_SYSTEM_PACKAGE + OFF + PACKAGE_NAME + ccd + LIBRARY_NAME + ccd + GIT_REPO + https://github.com/danfis/libccd.git + GIT_TAG + ${MUJOCO_DEP_VERSION_ccd} + TARGETS + ccd + EXCLUDE_FROM_ALL +) +target_compile_options(ccd PRIVATE ${MUJOCO_MACOS_COMPILE_OPTIONS}) +target_link_options(ccd PRIVATE ${MUJOCO_MACOS_LINK_OPTIONS}) + +# libCCD has an unconditional `#define _CRT_SECURE_NO_WARNINGS` on Windows. +# TODO(stunya): Remove this after https://github.com/danfis/libccd/pull/77 is merged. +if(WIN32) + if(MSVC) + # C4005 is the MSVC equivalent of -Wmacro-redefined. + target_compile_options(ccd PRIVATE /wd4005) + else() + target_compile_options(ccd PRIVATE -Wno-macro-redefined) + endif() +endif() + +if(MUJOCO_BUILD_TESTS) + set(ABSL_PROPAGATE_CXX_STD ON) + + # This specific version of Abseil does not have the following variable. We need to work with BUILD_TESTING + set(BUILD_TESTING_OLD ${BUILD_TESTING}) + set(BUILD_TESTING + OFF + CACHE INTERNAL "Build tests." + ) + + set(ABSL_BUILD_TESTING OFF) + findorfetch( + USE_SYSTEM_PACKAGE + OFF + PACKAGE_NAME + absl + LIBRARY_NAME + abseil-cpp + GIT_REPO + https://github.com/abseil/abseil-cpp.git + GIT_TAG + ${MUJOCO_DEP_VERSION_abseil} + TARGETS + absl::core_headers + EXCLUDE_FROM_ALL + ) + + set(BUILD_TESTING + ${BUILD_TESTING_OLD} + CACHE BOOL "Build tests." FORCE + ) + + # Avoid linking errors on Windows by dynamically linking to the C runtime. + set(gtest_force_shared_crt + ON + CACHE BOOL "" FORCE + ) + + findorfetch( + USE_SYSTEM_PACKAGE + OFF + PACKAGE_NAME + GTest + LIBRARY_NAME + googletest + GIT_REPO + https://github.com/google/googletest.git + GIT_TAG + ${MUJOCO_DEP_VERSION_gtest} + TARGETS + gtest + gmock + gtest_main + EXCLUDE_FROM_ALL + ) + + set(BENCHMARK_EXTRA_FETCH_ARGS "") + if(WIN32 AND NOT MSVC) + set(BENCHMARK_EXTRA_FETCH_ARGS + PATCH_COMMAND + "sed" + "-i" + "-e" + "s/-std=c++11/-std=c++14/g" + "-e" + "s/HAVE_CXX_FLAG_STD_CXX11/HAVE_CXX_FLAG_STD_CXX14/g" + "${CMAKE_BINARY_DIR}/_deps/benchmark-src/CMakeLists.txt" + ) + endif() + + set(BENCHMARK_ENABLE_TESTING OFF) + + findorfetch( + USE_SYSTEM_PACKAGE + OFF + PACKAGE_NAME + benchmark + LIBRARY_NAME + benchmark + GIT_REPO + https://github.com/google/benchmark.git + GIT_TAG + ${MUJOCO_DEP_VERSION_benchmark} + TARGETS + benchmark::benchmark + benchmark::benchmark_main + ${BENCHMARK_EXTRA_FETCH_ARGS} + EXCLUDE_FROM_ALL + ) +endif() + +if(MUJOCO_TEST_PYTHON_UTIL) + add_compile_definitions(EIGEN_MPL2_ONLY) + if(NOT TARGET eigen) + # Support new IN_LIST if() operator. + set(CMAKE_POLICY_DEFAULT_CMP0057 NEW) + + FetchContent_Declare( + Eigen3 + GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git + GIT_TAG ${MUJOCO_DEP_VERSION_Eigen3} + ) + + FetchContent_GetProperties(Eigen3) + if(NOT Eigen3_POPULATED) + FetchContent_Populate(Eigen3) + + # Mark the library as IMPORTED as a workaround for https://gitlab.kitware.com/cmake/cmake/-/issues/15415 + add_library(Eigen3::Eigen INTERFACE IMPORTED) + set_target_properties( + Eigen3::Eigen PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${eigen3_SOURCE_DIR}" + ) + endif() + endif() +endif() + +# Reset BUILD_SHARED_LIBS to its previous value +set(BUILD_SHARED_LIBS + ${BUILD_SHARED_LIBS_OLD} + CACHE BOOL "Build MuJoCo as a shared library" FORCE +) diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoHarden.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoHarden.cmake new file mode 100644 index 0000000..7beb88f --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoHarden.cmake @@ -0,0 +1,35 @@ +# Copyright 2022 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +option(MUJOCO_HARDEN "Enable build hardening for MuJoCo." OFF) +if(MUJOCO_HARDEN + AND NOT + CMAKE_CXX_COMPILER_ID + MATCHES + ".*Clang.*" +) + message(FATAL_ERROR "MUJOCO_HARDEN is only supported when building with Clang") +endif() + +if(MUJOCO_HARDEN) + set(MUJOCO_HARDEN_COMPILE_OPTIONS -D_FORTIFY_SOURCE=2 -fstack-protector) + if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + set(MUJOCO_HARDEN_LINK_OPTIONS -Wl,-bind_at_load) + elseif(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + set(MUJOCO_HARDEN_LINK_OPTIONS -Wl,-z,relro -Wl,-z,now) + endif() +else() + set(MUJOCO_HARDEN_COMPILE_OPTIONS "") + set(MUJOCO_HARDEN_LINK_OPTIONS "") +endif() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoLinkOptions.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoLinkOptions.cmake new file mode 100644 index 0000000..242767f --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoLinkOptions.cmake @@ -0,0 +1,67 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +include(CheckCSourceCompiles) + +# Gets the appropriate linker options for building MuJoCo, based on features available on the +# linker. +function(get_mujoco_extra_link_options OUTPUT_VAR) + if(MSVC) + set(EXTRA_LINK_OPTIONS /OPT:REF /OPT:ICF=5) + else() + set(EXTRA_LINK_OPTIONS) + + if(WIN32) + set(CMAKE_REQUIRED_FLAGS "-fuse-ld=lld-link") + check_c_source_compiles("int main() {}" SUPPORTS_LLD) + if(SUPPORTS_LLD) + set(EXTRA_LINK_OPTIONS + ${EXTRA_LINK_OPTIONS} + -fuse-ld=lld-link + -Wl,/OPT:REF + -Wl,/OPT:ICF + ) + endif() + else() + set(CMAKE_REQUIRED_FLAGS "-fuse-ld=lld") + check_c_source_compiles("int main() {}" SUPPORTS_LLD) + if(SUPPORTS_LLD) + set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -fuse-ld=lld) + else() + set(CMAKE_REQUIRED_FLAGS "-fuse-ld=gold") + check_c_source_compiles("int main() {}" SUPPORTS_GOLD) + if(SUPPORTS_GOLD) + set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -fuse-ld=gold) + endif() + endif() + + set(CMAKE_REQUIRED_FLAGS ${EXTRA_LINK_OPTIONS} "-Wl,--gc-sections") + check_c_source_compiles("int main() {}" SUPPORTS_GC_SECTIONS) + if(SUPPORTS_GC_SECTIONS) + set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -Wl,--gc-sections) + else() + set(CMAKE_REQUIRED_FLAGS ${EXTRA_LINK_OPTIONS} "-Wl,-dead_strip") + check_c_source_compiles("int main() {}" SUPPORTS_DEAD_STRIP) + if(SUPPORTS_DEAD_STRIP) + set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} -Wl,-dead_strip) + endif() + endif() + endif() + endif() + + set("${OUTPUT_VAR}" + ${EXTRA_LINK_OPTIONS} + PARENT_SCOPE + ) +endfunction() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoMacOS.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoMacOS.cmake new file mode 100644 index 0000000..8ff7cd3 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoMacOS.cmake @@ -0,0 +1,41 @@ +# Copyright 2022 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +if(APPLE) + # Target the oldest version of macOS that is still supported by Apple. + # We follow https://endoflife.date/macos, which considers a version to become + # unsupported the first time it is excluded from a macOS security update + # (e.g. https://github.com/endoflife-date/endoflife.date/issues/1602). + set(MUJOCO_MACOSX_VERSION_MIN 11) + + # We are setting the -mmacosx-version-min compiler flag directly rather than using the + # CMAKE_OSX_DEPLOYMENT_TARGET variable since we do not want to affect choice of SDK, + # and also we only want to apply the version restriction locally. + set(MUJOCO_MACOS_COMPILE_OPTIONS -mmacosx-version-min=${MUJOCO_MACOSX_VERSION_MIN} + -Werror=partial-availability -Werror=unguarded-availability + ) + set(MUJOCO_MACOS_LINK_OPTIONS -mmacosx-version-min=${MUJOCO_MACOSX_VERSION_MIN} + -Wl,-no_weak_imports + ) +else() + set(MUJOCO_MACOS_COMPILE_OPTIONS "") + set(MUJOCO_MACOS_LINK_OPTIONS "") +endif() + +function(enforce_mujoco_macosx_min_version) + if(APPLE) + add_compile_options(${MUJOCO_MACOS_COMPILE_OPTIONS}) + add_link_options(${MUJOCO_MACOS_LINK_OPTIONS}) + endif() +endfunction() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoOptions.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoOptions.cmake new file mode 100644 index 0000000..3e3f080 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/MujocoOptions.cmake @@ -0,0 +1,113 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +# Global compilation settings +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_C_EXTENSIONS OFF) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # For LLVM tooling + +if(NOT CMAKE_CONFIGURATION_TYPES) + if(NOT CMAKE_BUILD_TYPE) + message(STATUS "Setting build type to 'Release' as none was specified.") + set(CMAKE_BUILD_TYPE + "Release" + CACHE STRING "Choose the type of build, recommanded options are: Debug or Release" FORCE + ) + endif() + set(BUILD_TYPES + "Debug" + "Release" + "MinSizeRel" + "RelWithDebInfo" + ) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${BUILD_TYPES}) +endif() + +include(GNUInstallDirs) + +# Change the default output directory in the build structure. This is not stricly needed, but helps +# running in Windows, such that all built executables have DLLs in the same folder as the .exe +# files. +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") + +set(OpenGL_GL_PREFERENCE GLVND) + +set(CMAKE_POSITION_INDEPENDENT_CODE ON) +set(CMAKE_C_VISIBILITY_PRESET hidden) +set(CMAKE_CXX_VISIBILITY_PRESET hidden) +set(CMAKE_VISIBILITY_INLINES_HIDDEN ON) + +if(MSVC) + add_compile_options(/Gy /Gw /Oi) +elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-fdata-sections -ffunction-sections) +endif() + +# We default to shared library. +set(BUILD_SHARED_LIBS + ON + CACHE BOOL "Build Mujoco as shared library." +) + +option(MUJOCO_ENABLE_AVX "Build binaries that require AVX instructions, if possible." ON) +option(MUJOCO_ENABLE_AVX_INTRINSICS "Make use of hand-written AVX intrinsics, if possible." ON) +option(MUJOCO_ENABLE_RPATH "Enable RPath support when installing Mujoco." ON) +mark_as_advanced(MUJOCO_ENABLE_RPATH) + +if(MUJOCO_ENABLE_AVX) + include(CheckAvxSupport) + get_avx_compile_options(AVX_COMPILE_OPTIONS) +else() + set(AVX_COMPILE_OPTIONS) +endif() + +option(MUJOCO_BUILD_MACOS_FRAMEWORKS "Build libraries as macOS Frameworks" OFF) + +# Get some extra link options. +include(MujocoLinkOptions) +get_mujoco_extra_link_options(EXTRA_LINK_OPTIONS) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang" AND NOT MSVC)) + set(EXTRA_COMPILE_OPTIONS + -Werror + -Wall + -Wpedantic + -Wimplicit-fallthrough + -Wunused + -Wvla + -Wno-int-in-bool-context + -Wno-sign-compare + -Wno-unknown-pragmas + ) + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + # Set -Wimplicit-fallthrough=5 to only allow fallthrough annotation via __attribute__. + set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} -Wimplicit-fallthrough=5 + -Wno-maybe-uninitialized + ) + endif() +endif() + +include(MujocoHarden) +set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS}) +set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS}) + +if(WIN32) + add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_DEPRECATE) +endif() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/ShellTests.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/ShellTests.cmake new file mode 100644 index 0000000..b26b165 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/ShellTests.cmake @@ -0,0 +1,52 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +function(add_mujoco_shell_test TEST_NAME TARGET_BINARY) + find_program(BASH_PROGRAM bash) + if(BASH_PROGRAM) + # Set up the test directory + set(TEST_TMPDIR "${CMAKE_CURRENT_BINARY_DIR}/${TEST_NAME}") + add_test(NAME ${TEST_NAME}_setup + COMMAND ${BASH_PROGRAM} "${PROJECT_SOURCE_DIR}/cmake/setup_test_dir.sh" ${TEST_TMPDIR} + ) + set_tests_properties(${TEST_NAME}_setup PROPERTIES FIXTURES_SETUP ${TEST_NAME}_fixture) + add_test(NAME ${TEST_NAME}_cleanup + COMMAND ${BASH_PROGRAM} "${PROJECT_SOURCE_DIR}/cmake/cleanup_test_dir.sh" + ${TEST_TMPDIR} + ) + set_tests_properties(${TEST_NAME}_cleanup PROPERTIES FIXTURES_CLEANUP ${TEST_NAME}_fixture) + add_test( + NAME ${TEST_NAME} + COMMAND ${BASH_PROGRAM} "${CMAKE_CURRENT_SOURCE_DIR}/${TEST_NAME}.sh" + WORKING_DIRECTORY $ + ) + set_tests_properties(${TEST_NAME} PROPERTIES FIXTURES_REQUIRED ${TEST_NAME}_fixture) + set_property( + TEST "${TEST_NAME}" + PROPERTY ENVIRONMENT + "CMAKE_SOURCE_DIR=${CMAKE_SOURCE_DIR}" + "TARGET_BINARY=$" + "TEST_TMPDIR=${TEST_TMPDIR}" + ) + if(WIN32) + # Define the directory containing the mujoco DLL library so that it can be added to the PATH. + # We modify the PATH in the script as it is more reliable. + set_property( + TEST "${TEST_NAME}" + APPEND + PROPERTY ENVIRONMENT "MUJOCO_DLL_DIR=$" + ) + endif() + endif() +endfunction() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/TargetAddRpath.cmake b/locomotion/src/robot_simulation/third_party/mujoco/cmake/TargetAddRpath.cmake new file mode 100644 index 0000000..0354223 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/TargetAddRpath.cmake @@ -0,0 +1,189 @@ +# Copyright 2022 DeepMind Technologies Limited +# +# 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 +# +# https://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. +# +#[=======================================================================[.rst: +TargetAddRpath +---------------------- + +Add support to RPATH for the specified targets + +.. command:: target_add_rpath + + Add support to RPATH for the specified targets:: + +.. code-block:: cmake + target_add_rpath(target1 target2 ... + INSTALL_DIRECTORY install_directory + LIB_DIRS dir1 dir2 ... + [INSTALL_NAME_DIR [dir]] + [DEPENDS condition [condition]] + [USE_LINK_PATH]) + + This function setups the RPATH paths for the specified targets. The use of + RPATH allows to avoid using the (dangerous) environment variable + ``LD_LIBRARY_PATH`` (or equivalent on macOS) when installing without absolute + paths. + By using RPATH the installation can be relocated as the linker will look for + (some of) the dependencies at run time. + + The function has the following parameters: + + Options: + - ``USE_LINK_PATH``: if defined, the function will set + ``INSTALL_RPATH_USE_LINK_PATH`` on all the specified targets, i.e. CMake + will automatically adds to the RPATH the path to all the dependent + libraries defined outside this project. + + Arguments: + - ``INSTALL_DIRECTORY`` The directory where the specified targets will be + installed. + - ``LIB_DIRS`` list of directories to be added a search path to the RPATH. + Note that the relative path between ``INSTALL_DIRECTORY`` and these + directories will be added to the RPATH. + - ``INSTALL_NAME_DIR`` directory where the libraries will be installed. + This variable will be used only if ``CMAKE_SKIP_RPATH`` or + ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the + ``INSTALL_NAME_DIR`` on all targets. + - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable + RPATH, for example ``FOO; NOT BAR``. + + Note: see https://gitlab.kitware.com/cmake/community/-/wikis/doc/cmake/RPATH-handling + and https://gitlab.kitware.com/cmake/cmake/issues/16589 for further details. + +#]=======================================================================] + +if(COMMAND target_add_rpath) + return() +endif() + +function(_get_system_dirs _output_var) + set(${_output_var} + ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES} + PARENT_SCOPE + ) +endfunction() + +function( + _get_rpath_relative_path + _output_var + _bin_dir + _lib_dir +) + file( + RELATIVE_PATH + _rel_path + ${_bin_dir} + ${_lib_dir} + ) + if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + set(${_output_var} + "@loader_path/${_rel_path}" + PARENT_SCOPE + ) + else() + set(${_output_var} + "\$ORIGIN/${_rel_path}" + PARENT_SCOPE + ) + endif() +endfunction() + +function(target_add_rpath) + set(_options USE_LINK_PATH) + set(_oneValueArgs INSTALL_NAME_DIR INSTALL_DIRECTORY) + set(_multiValueArgs TARGETS LIB_DIRS DEPENDS) + + cmake_parse_arguments( + _ARGS + "${_options}" + "${_oneValueArgs}" + "${_multiValueArgs}" + "${ARGN}" + ) + + # Handle Apple-specific installation directory. Note that this disable proper RPATH. + if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH) + if(DEFINED _ARGS_INSTALL_NAME_DIR) + set_target_properties(${_ARGS_TARGETS} PROPERTIES INSTALL_NAME_DIR ${_ARGS_INSTALL_NAME_DIR}) + endif() + endif() + + # If RPATH is disabled, do nothing and return. + if(CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) + return() + endif() + + # Check if the user requested RPATH for the specified targets. + set(_enable_rpath ON) + if(DEFINED _ARGS_DEPENDS) + foreach(_cond ${_ARGS_DEPENDS}) + string( + REGEX + REPLACE " +" + ";" + _cond + "${_cond}" + ) + if(NOT (${_cond})) + set(_enable_rpath OFF) + endif() + endforeach() + endif() + + if(NOT _enable_rpath) + return() + endif() + + # Now enable RPATH for the specified targets. + _get_system_dirs(_system_dirs) + + # We do this per target to preserve the original rpath setting. + foreach(_target ${_ARGS_TARGETS}) + get_target_property(_install_rpath ${_target} INSTALL_RPATH) + + foreach(_lib_dir ${_ARGS_LIB_DIRS}) + # Check if the specified library path is a system directory. These are always searched so we do + # not need to include them in the RPATH. + list( + FIND + _system_dirs + "${_lib_dir}" + is_system_dir + ) + if("${is_system_dir}" STREQUAL "-1") + + _get_rpath_relative_path(_bin_lib_rel_path ${_ARGS_INSTALL_DIRECTORY} ${_lib_dir}) + list(APPEND _install_rpath ${_bin_lib_rel_path}) + + endif() + endforeach() + + if(NOT + "${_install_rpath}" + STREQUAL + "" + ) + list(REMOVE_DUPLICATES _install_rpath) + endif() + + set_target_properties( + ${_target} + PROPERTIES INSTALL_RPATH ${_install_rpath} + INSTALL_RPATH_USE_LINK_PATH ${_ARGS_USE_LINK_PATH} + BUILD_WITH_INSTALL_RPATH TRUE + MACOSX_RPATH ON # This is ON by default. + ) + endforeach() + +endfunction() diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/cleanup_test_dir.sh b/locomotion/src/robot_simulation/third_party/mujoco/cmake/cleanup_test_dir.sh new file mode 100755 index 0000000..ed333b3 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/cleanup_test_dir.sh @@ -0,0 +1,21 @@ +#!/bin/bash +# Copyright 2021 DeepMind Technologies Limited +# +# 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. + +if [ -z "$1" ]; then + echo "Expecting an output directory. Got none." 1>&2 + exit 1 +fi + +rm -rf "$1" diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/mujocoConfig.cmake.in b/locomotion/src/robot_simulation/third_party/mujoco/cmake/mujocoConfig.cmake.in new file mode 100644 index 0000000..d00d1cc --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/mujocoConfig.cmake.in @@ -0,0 +1,24 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# 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 +# +# https://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. + +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(OpenGL) + +if(NOT TARGET mujoco AND NOT @PROJECT_NAME@_BINARY_DIR) + include("${CMAKE_CURRENT_LIST_DIR}/mujocoTargets.cmake") +endif() + +check_required_components(mujoco) diff --git a/locomotion/src/robot_simulation/third_party/mujoco/cmake/setup_test_dir.sh b/locomotion/src/robot_simulation/third_party/mujoco/cmake/setup_test_dir.sh new file mode 100755 index 0000000..46ec06d --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/cmake/setup_test_dir.sh @@ -0,0 +1,22 @@ +#!/bin/bash +# Copyright 2021 DeepMind Technologies Limited +# +# 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. + +if [ -z "$1" ]; then + echo "Expecting an output directory. Got none." 1>&2 + exit 1 +fi + +mkdir -p "$1" +rm -rf "$1/*" diff --git a/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APIfunctions.rst b/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APIfunctions.rst new file mode 100644 index 0000000..81a01e6 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APIfunctions.rst @@ -0,0 +1,113 @@ +.. _API: + +========= +Functions +========= + +The main header `mujoco.h `_ exposes a +large number of functions. However the functions that most users are likely to need are a small fraction. + +API function can be classified as: + +- :ref:`Parse and compile` an :ref:`mjModel` from XML files and assets. +- :ref:`Main simulation` entry points, including :ref:`mj_step`. +- :ref:`Support` functions requiring :ref:`mjModel` and :ref:`mjData`. +- :ref:`Components` of the simulation pipeline, called from :ref:`mj_step`, :ref:`mj_forward` and :ref:`mj_inverse`. +- :ref:`Sub components` of the simulation pipeline. +- :ref:`Ray collisions`. +- :ref:`Printing` of various quantities. +- :ref:`Virtual file system`, used to load assets from memory. +- :ref:`Initialization` of data structures. +- :ref:`Abstract interaction`: mouse control of cameras and perturbations. +- :ref:`Abstract Visualization`. +- :ref:`OpenGL rendering`. +- :ref:`UI framework`. +- :ref:`Error and memory`. +- :ref:`Aliases for C standard math` functions. +- :ref:`Vector math`. +- :ref:`Quaternions`. +- :ref:`Poses transformations`. +- :ref:`Matrix decompositions and solvers`. +- :ref:`Miscellaneous` functions. +- :ref:`Derivatives`. +- :ref:`Plugin` related functions. +- :ref:`Macros`. +- :ref:`Thread` related functions. + +.. TODO(b/273075045): Better category-label namespacing. + +.. include:: functions.rst + +.. _Macros: + +Macros +^^^^^^ + +.. _mjDISABLED: + +mjDISABLED +~~~~~~~~~~ + +.. code-block:: C + + #define mjDISABLED(x) (m->opt.disableflags & (x)) + +Check if a given standard feature has been disabled via the physics options, assuming mjModel\* m is defined. x is of +type :ref:`mjtDisableBit`. + + +.. _mjENABLED: + +mjENABLED +~~~~~~~~~ + +.. code-block:: C + + #define mjENABLED(x) (m->opt.enableflags & (x)) + +Check if a given optional feature has been enabled via the physics options, assuming mjModel\* m is defined. x is of +type :ref:`mjtEnableBit`. + + +.. _mjMAX: + +mjMAX +~~~~~ + +.. code-block:: C + + #define mjMAX(a,b) (((a) > (b)) ? (a) : (b)) + +Return maximum value. To avoid repeated evaluation with mjtNum types, use the function :ref:`mju_max`. + + +.. _mjMIN: + +mjMIN +~~~~~ + +.. code-block:: C + + #define mjMIN(a,b) (((a) < (b)) ? (a) : (b)) + +Return minimum value. To avoid repeated evaluation with mjtNum types, use the function :ref:`mju_min`. + + +.. _mjPLUGIN_LIB_INIT: + +mjPLUGIN_LIB_INIT +~~~~~~~~~~~~~~~~~ + +.. code-block:: C + + #define mjPLUGIN_LIB_INIT \ + static void _mjplugin_dllmain(void); \ + mjEXTERNC int __stdcall mjDLLMAIN(void* hinst, unsigned long reason, void* reserved) { \ + if (reason == 1) { \ + _mjplugin_dllmain(); \ + } \ + return 1; \ + } \ + static void _mjplugin_dllmain(void) + +Register a plugin as a dynamic library. See :ref:`plugin registration` for more details. diff --git a/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APIglobals.rst b/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APIglobals.rst new file mode 100644 index 0000000..53ea109 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APIglobals.rst @@ -0,0 +1,540 @@ +======= +Globals +======= + +Global variable and constant definitions can be classified as: + +- Callbacks: + + - :ref:`glError`. + - :ref:`glMemory`. + - :ref:`glPhysics`. + +- The :ref:`collision table` containing narrow-phase collision functions. +- :ref:`String constants`. +- :ref:`Numeric constants`. +- :ref:`X Macros`. + +.. _glError: + +Error callbacks +^^^^^^^^^^^^^^^ + +All user callbacks (i.e., global function pointers whose name starts with 'mjcb') are initially set to NULL, which +disables them and allows the default processing to take place. To install a callback, simply set the corresponding +global pointer to a user function of the correct type. Keep in mind that these are global and not model-specific. So if +you are simulating multiple models in parallel, they use the same set of callbacks. + + +.. _mju_user_error: + +mju_user_error +~~~~~~~~~~~~~~ + +This is called from within the main error function :ref:`mju_error`. When installed, this function overrides the default +error processing. Once it prints error messages (or whatever else the user wants to do), it must **exit** the program. +MuJoCo is written with the assumption that mju_error will not return. If it does, the behavior of the software is +undefined. + +.. code-block:: C + + extern void (*mju_user_error)(const char*); + + +.. _mju_user_warning: + +mju_user_warning +~~~~~~~~~~~~~~~~ + +This is called from within the main warning function :ref:`mju_warning`. It is similar to the error handler, but instead +it must return without exiting the program. + +.. code-block:: C + + extern void (*mju_user_warning)(const char*); + + +.. _glMemory: + +Memory callbacks +^^^^^^^^^^^^^^^^ + +The purpose of the memory callbacks is to allow the user to install custom memory allocation and deallocation +mechanisms. One example where we have found this to be useful is a MATLAB wrapper for MuJoCo, where mex files are +expected to use MATLAB's memory mechanism for permanent memory allocation. + + +.. _mju_user_malloc: + +mju_user_malloc +~~~~~~~~~~~~~~~ + +If this is installed, the MuJoCo runtime will use it to allocate all heap memory it needs (instead of using aligned +malloc). The user allocator must allocate memory aligned on 8-byte boundaries. Note that the parser and compiler are +written in C++ and sometimes allocate memory with the "new" operator which bypasses this mechanism. + +.. code-block:: C + + extern void* (*mju_user_malloc)(size_t); + + +.. _mju_user_free: + +mju_user_free +~~~~~~~~~~~~~ + +If this is installed, MuJoCo will free any heap memory it allocated by calling this function (instead of using aligned +free). + +.. code-block:: C + + extern void (*mju_user_free)(void*); + + +.. _glPhysics: + +Physics callbacks +^^^^^^^^^^^^^^^^^ + +The physics callbacks are the main mechanism for modifying the behavior of the simulator, beyond setting various +options. The options control the operation of the default pipeline, while callbacks extend the pipeline at +well-defined places. This enables advanced users to implement many interesting functions which we have not thought of, +while still taking advantage of the default pipeline. As with all other callbacks, there is no automated error +checking - instead we assume that the authors of callback functions know what they are doing. + +Custom physics callbacks will often need parameters that are not standard in MJCF. This is largely why we have +provided custom fields as well as user data arrays in MJCF. The idea is to "instrument" the MJCF model by entering the +necessary user parameters, and then write callbacks that look for those parameters and perform the corresponding +computations. We strongly encourage users to write callbacks that check the model for the presence of user parameters +before accessing them - so that when a regular model is loaded, the callback disables itself automatically instead of +causing the software to crash. + +.. _mjcb_passive: + +mjcb_passive +~~~~~~~~~~~~ + +This is used to implement a custom passive force in joint space; if the force is more naturally defined in Cartesian +space, use the end-effector Jacobian to map it to joint space. By "passive" we do not mean a force that does no positive +work (as in physics), but simply a force that depends only on position and velocity but not on control. There are +standard passive forces in MuJoCo arising from springs, dampers, viscosity and density of the medium. They are computed +in ``mjData.qfrc_passive`` before mjcb_passive is called. The user callback should add to this vector instead of +overwriting it (otherwise the standard passive forces will be lost). + +.. code-block:: C + + extern mjfGeneric mjcb_passive; + + +.. _mjcb_control: + +mjcb_control +~~~~~~~~~~~~ + +This is the most commonly used callback. It implements a control law, by writing in the vector of controls +``mjData.ctrl``. It can also write in ``mjData.qfrc_applied`` and ``mjData.xfrc_applied``. The values written in these +vectors can depend on position, velocity and all other quantities derived from them, but cannot depend on contact forces +and other quantities that are computed after the control is specified. If the callback accesses the latter fields, their +values do not correspond to the current time step. + +The control callback is called from within :ref:`mj_forward` and :ref:`mj_step`, just before the controls and applied +forces are needed. When using the RK integrator, it will be called 4 times per step. The alternative way of specifying +controls and applied forces is to set them before ``mj_step``, or use ``mj_step1`` and ``mj_step2``. The latter approach +allows setting the controls after the position and velocity computations have been performed by ``mj_step1``, allowing +these results to be utilized in computing the control (similar to using mjcb_control). However, the only way to change +the controls between sub-steps of the RK integrator is to define the control callback. + +.. code-block:: C + + extern mjfGeneric mjcb_control; + +.. _mjcb_contactfilter: + +mjcb_contactfilter +~~~~~~~~~~~~~~~~~~ + +This callback can be used to replace MuJoCo's default collision filtering. When installed, this function is called for +each pair of geoms that have passed the broad-phase test (or are predefined geom pairs in the MJCF) and are candidates +for near-phase collision. The default processing uses the contype and conaffinity masks, the parent-child filter and +some other considerations related to welded bodies to decide if collision should be allowed. This callback replaces the +default processing, but keep in mind that the entire mechanism is being replaced. So for example if you still want to +take advantage of contype/conaffinity, you have to re-implement it in the callback. + +.. code-block:: C + + extern mjfConFilt mjcb_contactfilter; + +.. _mjcb_sensor: + +mjcb_sensor +~~~~~~~~~~~ + +This callback populates fields of ``mjData.sensordata`` corresponding to user-defined sensors. It is called if it is +installed and the model contains user-defined sensors. It is called once per compute stage (mjSTAGE_POS, mjSTAGE_VEL, +mjSTAGE_ACC) and must fill in all user sensor values for that stage. The user-defined sensors have dimensionality and +data types defined in the MJCF model which must be respected by the callback. + +.. code-block:: C + + extern mjfSensor mjcb_sensor; + +.. _mjcb_time: + +mjcb_time +~~~~~~~~~ + +Installing this callback enables the built-in profiler, and keeps timing statistics in ``mjData.timer``. The return type +is mjtNum, while the time units are up to the user. :ref:`simulate.cc ` assumes the unit is 1 millisecond. +In order to be useful, the callback should use high-resolution timers with at least microsecond precision. This is +because the computations being timed are very fast. + +.. code-block:: C + + extern mjfTime mjcb_time; + + +.. _mjcb_act_dyn: + +mjcb_act_dyn +~~~~~~~~~~~~ + +This callback implements custom activation dynamics: it must return the value of ``mjData.act_dot`` for the specified +actuator. This is the time-derivative of the activation state vector ``mjData.act``. It is called for model actuators +with user dynamics (mjDYN_USER). If such actuators exist in the model but the callback is not installed, their +time-derivative is set to 0. + +.. code-block:: C + + extern mjfAct mjcb_act_dyn; + + +.. _mjcb_act_gain: + +mjcb_act_gain +~~~~~~~~~~~~~ + +This callback implements custom actuator gains: it must return the gain for the specified actuator with +``mjModel.actuator_gaintype`` set to mjGAIN_USER. If such actuators exist in the model and this callback is not +installed, their gains are set to 1. + +.. code-block:: C + + extern mjfAct mjcb_act_gain; + + +.. _mjcb_act_bias: + +mjcb_act_bias +~~~~~~~~~~~~~ + +This callback implements custom actuator biases: it must return the bias for the specified actuator with +``mjModel.actuator_biastype`` set to mjBIAS_USER. If such actuators exist in the model and this callback is not +installed, their biases are set to 0. + +.. code-block:: C + + extern mjfAct mjcb_act_bias; + + +.. _glCollision: + +Collision table +^^^^^^^^^^^^^^^ + +.. _mjCOLLISIONFUNC: + +mjCOLLISIONFUNC +~~~~~~~~~~~~~~~ + +Table of pairwise collision functions indexed by geom types. Only the upper-right triangle is used. The user can replace +these function pointers with custom routines, replacing MuJoCo's collision mechanism. If a given entry is NULL, the +corresponding pair of geom types cannot be collided. Note that these functions apply only to near-phase collisions. The +broadphase mechanism is built-in and cannot be modified. + +.. code-block:: C + + extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES]; + + +.. _glString: + +String constants +^^^^^^^^^^^^^^^^ + +The string constants described here are provided for user convenience. They correspond to the English names of lists of +options, and can be displayed in menus or dialogs in a GUI. The code sample :ref:`simulate.cc ` illustrates +how they can be used. + + +.. _mjDISABLESTRING: + +mjDISABLESTRING +~~~~~~~~~~~~~~~ + +Names of the disable bits defined by :ref:`mjtDisableBit`. + +.. code-block:: C + + extern const char* mjDISABLESTRING[mjNDISABLE]; + + +.. _mjENABLESTRING: + +mjENABLESTRING +~~~~~~~~~~~~~~ + +Names of the enable bits defined by :ref:`mjtEnableBit`. + +.. code-block:: C + + extern const char* mjENABLESTRING[mjNENABLE]; + + +.. _mjTIMERSTRING: + +mjTIMERSTRING +~~~~~~~~~~~~~ + +Names of the mjData timers defined by :ref:`mjtTimer`. + +.. code-block:: C + + extern const char* mjTIMERSTRING[mjNTIMER]; + + +.. _mjLABELSTRING: + +mjLABELSTRING +~~~~~~~~~~~~~ + +Names of the visual labeling modes defined by :ref:`mjtLabel`. + +.. code-block:: C + + extern const char* mjLABELSTRING[mjNLABEL]; + + +.. _mjFRAMESTRING: + +mjFRAMESTRING +~~~~~~~~~~~~~ + +Names of the frame visualization modes defined by :ref:`mjtFrame`. + +.. code-block:: C + + extern const char* mjFRAMESTRING[mjNFRAME]; + + +.. _mjVISSTRING: + +mjVISSTRING +~~~~~~~~~~~ + +Descriptions of the abstract visualization flags defined by :ref:`mjtVisFlag`. For each flag there are three strings, + +with the following meaning: + +[0]: flag name; + +[1]: the string "0" or "1" indicating if the flag is on or off by default, as set by +:ref:`mjv_defaultOption`; + +[2]: one-character string with a suggested keyboard shortcut, used in :ref:`simulate.cc `. + +.. code-block:: C + + extern const char* mjVISSTRING[mjNVISFLAG][3]; + + +.. _mjRNDSTRING: + +mjRNDSTRING +~~~~~~~~~~~ + +Descriptions of the OpenGL rendering flags defined by :ref:`mjtRndFlag`. The three strings for each flag have the same +format as above, except the defaults here are set by :ref:`mjv_makeScene`. + +.. code-block:: C + + extern const char* mjRNDSTRING[mjNRNDFLAG][3]; + + + +.. _glNumeric: + +Numeric constants +^^^^^^^^^^^^^^^^^ + +Many integer constants were already documented in the primitive types above. In addition, the header files define +several other constants documented here. Unless indicated otherwise, each entry in the table below is defined in +`mjmodel.h `_. Note that some extended key +codes are defined in `mjui.h `_ which are not +shown in the table below. Their names are in the format ``mjKEY_XXX``. They correspond to GLFW key codes. + +.. list-table:: + :widths: 2 1 8 + :header-rows: 1 + + * - symbol + - value + - description + * - ``mjMINVAL`` + - 1E-15 + - The minimal value allowed in any denominator, and in general any mathematical operation where 0 is not allowed. + In almost all cases, MuJoCo silently clamps smaller values to mjMINVAL. + * - ``mjPI`` + - :math:`\pi` + - The value of :math:`\pi`. This is used in various trigonometric functions, and also for conversion from degrees + to radians in the compiler. + * - ``mjMAXVAL`` + - 1E+10 + - The maximal absolute value allowed in mjData.qpos, mjData.qvel, mjData.qacc. The API functions + :ref:`mj_checkPos`, :ref:`mj_checkVel`, :ref:`mj_checkAcc` use this constant to detect instability. + * - ``mjMINMU`` + - 1E-5 + - The minimal value allowed in any friction coefficient. Recall that MuJoCo's contact model allows different number + of friction dimensions to be included, as specified by the :at:`condim` attribute. If however a given friction + dimension is included, its friction is not allowed to be smaller than this constant. Smaller values are + automatically clamped to this constant. + * - ``mjMINIMP`` + - 0.0001 + - The minimal value allowed in any constraint impedance. Smaller values are automatically clamped to this constant. + * - ``mjMAXIMP`` + - 0.9999 + - The maximal value allowed in any constraint impedance. Larger values are automatically clamped to this constant. + * - ``mjMAXCONPAIR`` + - 50 + - The maximal number of contacts points that can be generated per geom pair. MuJoCo's built-in collision functions + respect this limit, and user-defined functions should also respect it. Such functions are called with a return + buffer of size ``mjMAXCONPAIR``; attempting to write more contacts in the buffer can cause unpredictable + behavior. + * - ``mjMAXTREEDEPTH`` + - 50 + - The maximum depth of each body and mesh bounding volume hierarchy. If this large limit is exceeded, a warning + is raised and ray casting may not be possible. For a balanced hierarchy, this implies 1E15 bounding volumes. + * - ``mjMAXVFS`` + - 200 + - The maximal number of characters in the name of each file in the virtual file system. + * - ``mjMAXVFSNAME`` + - 100 + - The maximal number of characters in the name of each file in the virtual file system. + * - ``mjNEQDATA`` + - 11 + - The maximal number of real-valued parameters used to define each equality constraint. Determines the size of + ``mjModel.eq_data``. This and the next five constants correspond to array sizes which we have not fully settled. + There may be reasons to increase them in the future, so as to accommodate extra parameters needed for more + elaborate computations. This is why we maintain them as symbolic constants that can be easily changed, as opposed + to the array size for representing quaternions for example -- which has no reason to change. + * - ``mjNDYN`` + - 10 + - The maximal number of real-valued parameters used to define the activation dynamics of each actuator. + Determines the size of ``mjModel.actuator_dynprm``. + * - ``mjNGAIN`` + - 10 + - The maximal number of real-valued parameters used to define the gain of each actuator. + Determines the size of ``mjModel.actuator_gainprm``. + * - ``mjNBIAS`` + - 10 + - The maximal number of real-valued parameters used to define the bias of each actuator. + Determines the size of ``mjModel.actuator_biasprm``. + * - ``mjNFLUID`` + - 12 + - The number of per-geom fluid interaction parameters required by the ellipsoidal model. + * - ``mjNREF`` + - 2 + - The maximal number of real-valued parameters used to define the reference acceleration of each scalar constraint. + Determines the size of all ``mjModel.XXX_solref`` fields. + * - ``mjNIMP`` + - 5 + - The maximal number of real-valued parameters used to define the impedance of each scalar constraint. + Determines the size of all ``mjModel.XXX_solimp`` fields. + * - ``mjNSOLVER`` + - 200 + - The number of iterations where solver statistics can be stored in ``mjData.solver``. This array is used + to store diagnostic information about each iteration of the constraint solver. + The actual number of iterations is given by ``mjData.solver_iter``. + * - ``mjNISLAND`` + - 20 + - The number of islands for which solver statistics can be stored in ``mjData.solver``. This array is + used to store diagnostic information about each iteration of the constraint solver. + The actual number of islands for which the solver was run is given by ``mjData.nsolver_island``. + * - ``mjNGROUP`` + - 6 + - The number of geom, site, joint, tendon and actuator groups whose rendering can be enabled and disabled via + :ref:`mjvOption`. + Defined in `mjvisualize.h `_. + * - ``mjMAXOVERLAY`` + - 500 + - The maximal number of characters in overlay text for rendering. + Defined in `mjvisualize.h `_. + * - ``mjMAXLINE`` + - 100 + - The maximal number of lines per 2D figure (:ref:`mjvFigure`). + Defined in `mjvisualize.h `_. + * - ``mjMAXLINEPNT`` + - 1000 + - The maximal number of points in each line in a 2D figure. Note that the buffer ``mjvFigure.linepnt`` has length + ``2*mjMAXLINEPNT`` because each point has X and Y coordinates. + Defined in `mjvisualize.h `_. + * - ``mjMAXPLANEGRID`` + - 200 + - The maximal number of grid lines in each dimension for rendering planes. + Defined in `mjvisualize.h `_. + * - ``mjNAUX`` + - 10 + - Number of auxiliary buffers that can be allocated in mjrContext. + Defined in `mjrender.h `_. + * - ``mjMAXTEXTURE`` + - 1000 + - Maximum number of textures allowed. + Defined in `mjrender.h `_. + * - ``mjMAXTHREAD`` + - 128 + - Maximum number OS threads that can be used in a thread pool. + Defined in `mjthread.h `_. + * - ``mjMAXUISECT`` + - 10 + - Maximum number of UI sections. + Defined in `mjui.h `_. + * - ``mjMAXUIITEM`` + - 200 + - Maximum number of items per UI section. + Defined in `mjui.h `_. + * - ``mjMAXUITEXT`` + - 500 + - Maximum number of characters in UI fields 'edittext' and 'other'. + Defined in `mjui.h `_. + * - ``mjMAXUINAME`` + - 40 + - Maximum number of characters in any UI name. + Defined in `mjui.h `_. + * - ``mjMAXUIMULTI`` + - 20 + - Maximum number of radio and select items in UI group. + Defined in `mjui.h `_. + * - ``mjMAXUIEDIT`` + - 5 + - Maximum number of elements in UI edit list. + Defined in `mjui.h `_. + * - ``mjMAXUIRECT`` + - 15 + - Maximum number of UI rectangles. + Defined in `mjui.h `_. + * - ``mjVERSION_HEADER`` + - 313 + - The version of the MuJoCo headers; changes with every release. This is an integer equal to 100x the software + version, so 210 corresponds to version 2.1. Defined in mujoco.h. The API function :ref:`mj_version` returns a + number with the same meaning but for the compiled library. + + +.. _tyXMacro: + +X Macros +^^^^^^^^ + +The X Macros are not needed in most user projects. They are used internally to allocate the model, and are also +available for users who know how to use this programming technique. See the header file `mjxmacro.h +`_ for the actual definitions. They are +particularly useful in writing MuJoCo wrappers for scripting languages, where dynamic structures matching the MuJoCo +data structures need to be constructed programmatically. diff --git a/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APItypes.rst b/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APItypes.rst new file mode 100644 index 0000000..ee8de50 --- /dev/null +++ b/locomotion/src/robot_simulation/third_party/mujoco/doc/APIreference/APItypes.rst @@ -0,0 +1,1287 @@ +===== +Types +===== + +MuJoCo defines a large number of types: + +- Two :ref:`primitive types`. +- :ref:`C enum types` used to define categorical values. These can be classified as: + + - Enums used in :ref:`mjModel`. + - Enums used in :ref:`mjData`. + - Abstract :ref:`visualization enums`. + - Enums used by the :ref:`openGL renderer`. + - Enums used by the :ref:`mjUI` user interface package. + + Note that the API does not use these enum types directly. Instead it uses ints, and the documentation/comments state + that certain ints correspond to certain enum types. This is because we want the API to be compiler-independent, and + the C standard does not dictate how many bytes must be used to represent an enum type. Nevertheless, for improved + readiblity, we recommend using these types when calling API functions which take them as arguments. + +- :ref:`C struct types`. These can be classified as: + + - Main structs: + + - :ref:`mjModel`. + - :ref:`mjOption` (embedded in :ref:`mjModel`). + - :ref:`mjData`. + + - :ref:`Auxillary struct types`, also used by the engine. + - Structs for collecting :ref:`simulation statistics`. + - Structs for :ref:`abstract visualization`. + - Structs used by the :ref:`openGL renderer`. + - Structs used by the :ref:`UI framework`. + - Structs used by :ref:`engine plugins`. + +- Several :ref:`tyFunction` for user-defined callbacks. +- :ref:`tyNotes` regarding specific data structures that require detailed description. + + + +.. _tyPrimitive: + +Primitive types +--------------- + +The two types below are defined in `mjtnum.h `_. + + +.. _mjtNum: + +mjtNum +^^^^^^ + +This is the floating-point type used throughout the simulator. If the symbol ``mjUSEDOUBLE`` is defined in +``mjmodel.h``, this type is defined as ``double``, otherwise it is defined as ``float``. Currently only the +double-precision version of MuJoCo is distributed, although the entire code base works with single-precision as well. +We may release the single-precision version in the future for efficiency reasons, but the double-precision version +will always be available. Thus it is safe to write user code assuming double precision. However, our preference is to +write code that works with either single or double precision. To this end we provide math utility functions that are +always defined with the correct floating-point type. + +Note that changing ``mjUSEDOUBLE`` in ``mjtnum.h`` will not change how the library was compiled, and instead will +result in numerous link errors. In general, the header files distributed with precompiled MuJoCo should never be +changed by the user. + +.. code-block:: C + + #ifdef mjUSEDOUBLE + typedef double mjtNum; + #else + typedef float mjtNum; + #endif + + +.. _mjtByte: + +mjtByte +^^^^^^^ + +Byte type used to represent boolean variables. + +.. code-block:: C + + typedef unsigned char mjtByte; + + +.. _tyEnums: + +Enum types +---------- + + +.. _tyModelEnums: + +Model +^^^^^ + +The enums below are defined in `mjmodel.h `_. + + +.. _mjtDisableBit: + +mjtDisableBit +~~~~~~~~~~~~~ + +Constants which are powers of 2. They are used as bitmasks for the field ``disableflags`` of :ref:`mjOption`. +At runtime this field is ``m->opt.disableflags``. The number of these constants is given by ``mjNDISABLE`` which is +also the length of the global string array :ref:`mjDISABLESTRING` with text descriptions of these flags. + +.. mujoco-include:: mjtDisableBit + + +.. _mjtEnableBit: + +mjtEnableBit +~~~~~~~~~~~~ + +Constants which are powers of 2. They are used as bitmasks for the field ``enableflags`` of :ref:`mjOption`. +At runtime this field is ``m->opt.enableflags``. The number of these constants is given by ``mjNENABLE`` which is also +the length of the global string array :ref:`mjENABLESTRING` with text descriptions of these flags. + +.. mujoco-include:: mjtEnableBit + + +.. _mjtJoint: + +mjtJoint +~~~~~~~~ + +Primitive joint types. These values are used in ``m->jnt_type``. The numbers in the comments indicate how many +positional coordinates each joint type has. Note that ball joints and rotational components of free joints are +represented as unit quaternions - which have 4 positional coordinates but 3 degrees of freedom each. + +.. mujoco-include:: mjtJoint + + +.. _mjtGeom: + +mjtGeom +~~~~~~~ + +Geometric types supported by MuJoCo. The first group are "official" geom types that can be used in the model. The +second group are geom types that cannot be used in the model but are used by the visualizer to add decorative +elements. These values are used in ``m->geom_type`` and ``m->site_type``. + +.. mujoco-include:: mjtGeom + + +.. _mjtCamLight: + +mjtCamLight +~~~~~~~~~~~ + +Dynamic modes for cameras and lights, specifying how the camera/light position and orientation are computed. These +values are used in ``m->cam_mode`` and ``m->light_mode``. + +.. mujoco-include:: mjtCamLight + + +.. _mjtTexture: + +mjtTexture +~~~~~~~~~~ + +Texture types, specifying how the texture will be mapped. These values are used in ``m->tex_type``. + +.. mujoco-include:: mjtTexture + + +.. _mjtIntegrator: + +mjtIntegrator +~~~~~~~~~~~~~ + +Numerical integrator types. These values are used in ``m->opt.integrator``. + +.. mujoco-include:: mjtIntegrator + +.. _mjtCone: + +mjtCone +~~~~~~~ + +Available friction cone types. These values are used in ``m->opt.cone``. + +.. mujoco-include:: mjtCone + +.. _mjtJacobian: + +mjtJacobian +~~~~~~~~~~~ + +Available Jacobian types. These values are used in ``m->opt.jacobian``. + +.. mujoco-include:: mjtJacobian + + +.. _mjtSolver: + +mjtSolver +~~~~~~~~~ + +Available constraint solver algorithms. These values are used in ``m->opt.solver``. + +.. mujoco-include:: mjtSolver + + +.. _mjtEq: + +mjtEq +~~~~~ + +Equality constraint types. These values are used in ``m->eq_type``. + +.. mujoco-include:: mjtEq + + +.. _mjtWrap: + +mjtWrap +~~~~~~~ + +Tendon wrapping object types. These values are used in ``m->wrap_type``. + +.. mujoco-include:: mjtWrap + + +.. _mjtTrn: + +mjtTrn +~~~~~~ + +Actuator transmission types. These values are used in ``m->actuator_trntype``. + +.. mujoco-include:: mjtTrn + + +.. _mjtDyn: + +mjtDyn +~~~~~~ + +Actuator dynamics types. These values are used in ``m->actuator_dyntype``. + +.. mujoco-include:: mjtDyn + + +.. _mjtGain: + +mjtGain +~~~~~~~ + +Actuator gain types. These values are used in ``m->actuator_gaintype``. + +.. mujoco-include:: mjtGain + + +.. _mjtBias: + +mjtBias +~~~~~~~ + +Actuator bias types. These values are used in ``m->actuator_biastype``. + +.. mujoco-include:: mjtBias + + +.. _mjtObj: + +mjtObj +~~~~~~ + +MuJoCo object types. These are used, for example, in the support functions :ref:`mj_name2id` and +:ref:`mj_id2name` to convert between object names and integer ids. + +.. mujoco-include:: mjtObj + + +.. _mjtConstraint: + +mjtConstraint +~~~~~~~~~~~~~ + +Constraint types. These values are not used in mjModel, but are used in the mjData field ``d->efc_type`` when the list +of active constraints is constructed at each simulation time step. + +.. mujoco-include:: mjtConstraint + +.. _mjtConstraintState: + +mjtConstraintState +~~~~~~~~~~~~~~~~~~ + +These values are used by the solver internally to keep track of the constraint states. + +.. mujoco-include:: mjtConstraintState + + +.. _mjtSensor: + +mjtSensor +~~~~~~~~~ + +Sensor types. These values are used in ``m->sensor_type``. + +.. mujoco-include:: mjtSensor + + +.. _mjtStage: + +mjtStage +~~~~~~~~ + +These are the compute stages for the skipstage parameters of :ref:`mj_forwardSkip` and +:ref:`mj_inverseSkip`. + +.. mujoco-include:: mjtStage + + +.. _mjtDataType: + +mjtDataType +~~~~~~~~~~~ + +These are the possible sensor data types, used in ``mjData.sensor_datatype``. + +.. mujoco-include:: mjtDataType + + + +.. _tyDataEnums: + +Data +^^^^ + +The enums below are defined in `mjdata.h `_. + + + +.. _mjtState: + +mjtState +~~~~~~~~ + +State component elements as integer bitflags and several convenient combinations of these flags. Used by +:ref:`mj_getState`, :ref:`mj_setState` and :ref:`mj_stateSize`. + +.. mujoco-include:: mjtState + + +.. _mjtWarning: + +mjtWarning +~~~~~~~~~~ + +Warning types. The number of warning types is given by ``mjNWARNING`` which is also the length of the array +``mjData.warning``. + +.. mujoco-include:: mjtWarning + + +.. _mjtTimer: + +mjtTimer +~~~~~~~~ + +Timer types. The number of timer types is given by ``mjNTIMER`` which is also the length of the array +``mjData.timer``, as well as the length of the string array :ref:`mjTIMERSTRING` with timer names. + +.. mujoco-include:: mjtTimer + + + +.. _tyVisEnums: + +Visualization +^^^^^^^^^^^^^ + +The enums below are defined in `mjvisualize.h `_. + + +.. _mjtCatBit: + +mjtCatBit +~~~~~~~~~ + +These are the available categories of geoms in the abstract visualizer. The bitmask can be used in the function +:ref:`mjr_render` to specify which categories should be rendered. + +.. mujoco-include:: mjtCatBit + + +.. _mjtMouse: + +mjtMouse +~~~~~~~~ + +These are the mouse actions that the abstract visualizer recognizes. It is up to the user to intercept mouse events +and translate them into these actions, as illustrated in :ref:`simulate.cc `. + +.. mujoco-include:: mjtMouse + + +.. _mjtPertBit: + +mjtPertBit +~~~~~~~~~~ + +These bitmasks enable the translational and rotational components of the mouse perturbation. For the regular mouse, +only one can be enabled at a time. For the 3D mouse (SpaceNavigator) both can be enabled simultaneously. They are used +in ``mjvPerturb.active``. + +.. mujoco-include:: mjtPertBit + + +.. _mjtCamera: + +mjtCamera +~~~~~~~~~ + +These are the possible camera types, used in ``mjvCamera.type``. + +.. mujoco-include:: mjtCamera + + +.. _mjtLabel: + +mjtLabel +~~~~~~~~ + +These are the abstract visualization elements that can have text labels. Used in ``mjvOption.label``. + +.. mujoco-include:: mjtLabel + + +.. _mjtFrame: + +mjtFrame +~~~~~~~~ + +These are the MuJoCo objects whose spatial frames can be rendered. Used in ``mjvOption.frame``. + +.. mujoco-include:: mjtFrame + + +.. _mjtVisFlag: + +mjtVisFlag +~~~~~~~~~~ + +These are indices in the array ``mjvOption.flags``, whose elements enable/disable the visualization of the +corresponding model or decoration element. + +.. mujoco-include:: mjtVisFlag + + +.. _mjtRndFlag: + +mjtRndFlag +~~~~~~~~~~ + +These are indices in the array ``mjvScene.flags``, whose elements enable/disable OpenGL rendering effects. + +.. mujoco-include:: mjtRndFlag + + +.. _mjtStereo: + +mjtStereo +~~~~~~~~~ + +These are the possible stereo rendering types. They are used in ``mjvScene.stereo``. + +.. mujoco-include:: mjtStereo + + + +.. _tyRenderEnums: + +Rendering +^^^^^^^^^ + +The enums below are defined in `mjrender.h `_. + + +.. _mjtGridPos: + +mjtGridPos +~~~~~~~~~~ + +These are the possible grid positions for text overlays. They are used as an argument to the function +:ref:`mjr_overlay`. + +.. mujoco-include:: mjtGridPos + + +.. _mjtFramebuffer: + +mjtFramebuffer +~~~~~~~~~~~~~~ + +These are the possible framebuffers. They are used as an argument to the function :ref:`mjr_setBuffer`. + +.. mujoco-include:: mjtFramebuffer + + +.. _mjtDepthMap: + +mjtDepthMap +~~~~~~~~~~~ + +These are the depth mapping options. They are used as a value for the ``readPixelDepth`` attribute of the +:ref:`mjrContext` struct, to control how the depth returned by :ref:`mjr_readPixels` is mapped from +``znear`` to ``zfar``. + +.. mujoco-include:: mjtDepthMap + + +.. _mjtFontScale: + +mjtFontScale +~~~~~~~~~~~~ + +These are the possible font sizes. The fonts are predefined bitmaps stored in the dynamic library at three different +sizes. + +.. mujoco-include:: mjtFontScale + + +.. _mjtFont: + +mjtFont +~~~~~~~ + +These are the possible font types. + +.. mujoco-include:: mjtFont + + +.. _tyUIEnums: + +User Interface +^^^^^^^^^^^^^^ + +The enums below are defined in `mjui.h `_. + + +.. _mjtButton: + +mjtButton +~~~~~~~~~ + +Mouse button IDs used in the UI framework. + +.. mujoco-include:: mjtButton + + +.. _mjtEvent: + +mjtEvent +~~~~~~~~ + +Event types used in the UI framework. + +.. mujoco-include:: mjtEvent + + +.. _mjtItem: + +mjtItem +~~~~~~~ + +Item types used in the UI framework. + +.. mujoco-include:: mjtItem + + +.. _tyPluginEnums: + +Plugins +^^^^^^^ + +The enums below are defined in `mjplugin.h `_. +See :ref:`exPlugin` for details. + + +.. _mjtPluginCapabilityBit: + +mjtPluginCapabilityBit +~~~~~~~~~~~~~~~~~~~~~~ + +Capabilities declared by an engine plugin. + +.. mujoco-include:: mjtPluginCapabilityBit + + + +.. _tyStructure: + +Struct types +------------ + +The three central struct types for physics simulation are :ref:`mjModel`, :ref:`mjOption` (embedded in :ref:`mjModel`) +and :ref:`mjData`. An introductory discussion of these strucures can be found in the :ref:`Overview`. + + +.. _mjModel: + +mjModel +^^^^^^^ + +This is the main data structure holding the MuJoCo model. It is treated as constant by the simulator. Some specific +details regarding datastructures in :ref:`mjModel` can be found below in :ref:`tyNotes`. + +.. mujoco-include:: mjModel + + + +.. _mjOption: + +mjOption +^^^^^^^^ + +This is the data structure with simulation options. It corresponds to the MJCF element +:ref:`option