optimo working in MuJoCo

shbang91 · Sep 15, 2024 · 5d62cc5 · 5d62cc5
1 parent 4131ef6
commit 5d62cc5
Show file tree

Hide file tree

Showing 19 changed files with 15,744 additions and 5 deletions.
diff --git a/config/optimo/INTERFACE.yaml b/config/optimo/INTERFACE.yaml
@@ -1,2 +1,2 @@
-test_env_name: pybullet # options: (1)pybullet (2)mujoco
+test_env_name: mujoco # options: (1)pybullet (2)mujoco
 whole_body_controller: ihwbc # options: (1)wbic (2)ihwbc
diff --git a/config/optimo/sim/mujoco/ihwbc/mujoco_params.yaml b/config/optimo/sim/mujoco/ihwbc/mujoco_params.yaml
@@ -0,0 +1,31 @@
+actuator_gains:
+  optimo_joint1:
+    kp: 0
+    kd: 0
+  optimo_joint2:
+    kp: 0
+    kd: 0
+  optimo_joint3:
+    kp: 0
+    kd: 0
+  optimo_joint4:
+    kp: 0
+    kd: 0
+  optimo_joint5:
+    kp: 0
+    kd: 0
+  optimo_joint6:
+    kp: 0
+    kd: 0
+  optimo_joint7:
+    kp: 0
+    kd: 0
+
+initial_config:
+  optimo_joint1: 0.
+  optimo_joint2: 3.14
+  optimo_joint3: 0.
+  optimo_joint4: -1.57
+  optimo_joint5: 0.
+  optimo_joint6: -1.57
+  optimo_joint7: 0.
diff --git a/controller/robot_system/pinocchio_robot_system.cpp b/controller/robot_system/pinocchio_robot_system.cpp
@@ -114,9 +114,7 @@ void PinocchioRobotSystem::_Initialize(
 }
 
 void PinocchioRobotSystem::_InitializeRootFrame() {
-  int joint_offset = 1; // pinocchio model always has universal joint
-  if (n_float_ != 0)
-    joint_offset = 2;
+  int joint_offset = 2; // pinocchio model always has universal joint
   root_frame_name_ = model_.frames[joint_offset].name;
   base_local_com_pos_ = model_.inertias[1].lever();
   std::cout << "=====================================" << std::endl;

diff --git a/robot_model/optimo/optimo.xml b/robot_model/optimo/optimo.xml
@@ -30,6 +30,7 @@
   </asset>
 
   <worldbody>
+  <body name="optimo_pedestal" pos="0 0 0">
     <geom size="0.15 0.15 0.5" type="box" rgba="1 1 1 1"/>
     <geom pos="0 0 0.77" quat="1 0 0 0" type="mesh" rgba="0.2 0.2 0.2 1" mesh="link0"/>
     <body name="optimo_link1" pos="0 0 0.77">
@@ -77,6 +78,7 @@
         </body>
       </body>
     </body>
+  </body>
   </worldbody>
 
   <actuator>

diff --git a/simulator/mujoco/CMakeLists.txt b/simulator/mujoco/CMakeLists.txt
@@ -1 +1,2 @@
 add_subdirectory(draco)
+add_subdirectory(optimo)
diff --git a/simulator/mujoco/optimo/CMakeLists.txt b/simulator/mujoco/optimo/CMakeLists.txt
@@ -0,0 +1,11 @@
+file(GLOB simulate_sources "simulate/*.cc" "simulate/*.cpp")
+file(GLOB simulate_headers "simulate/*.h")
+
+include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+add_executable(run_optimo main.cc ${simulate_sources} ${simulate_headers})
+target_link_libraries(run_optimo ${mujoco_LIBRARIES}
+                                ${glfw3_LIBRARIES}
+                                glfw
+                                Threads::Threads)
+target_link_libraries(run_optimo rpc-optimo-controller)
Original file line number	Diff line number	Diff line change
		@@ -1 +1,2 @@
		add_subdirectory(draco)
		add_subdirectory(optimo)