Mujoco sim upgrades

docs/source/development.md

@@ -52,16 +52,11 @@ While in the `pixi` shell and/or Docker container, all changes made in the repos
 ## Building and Running the ROS Stack
 All of the following commands should be run from within the `devcontainer` and within the `dev` pixi virtual environment (which can be entered by running `pixi shell -e dev`).
 
-Build all the messages:
+You can build eveerything with the terminal command:
 ```
-pixi run messages-build
+obk-build
 ```
-
-Build all the ROS packages (including the messages):
-```
-pixi run ros-build
-```
-***Note (as of 9/10/2024): if you have never built the workspace before, you will be required to build the messages first before running `ros-build`. This is because the messages are a dependency for the `ObeliskCpp` library and thus must be built first so they can be used. After you have built the ros workspace once, all future updates only require a `ros-build` command.***
+this will also be sure to build everything in the correct order.
 
 In a seperate terminal, we can run a ROS stack with:
 ```

docs/source/obelisk_terminal_aliases.md

@@ -12,6 +12,19 @@ Example:
 obk-launch config_file_path=example.yaml device_name=onboard auto_start=True
 ```
 
+## `obk-build`
+Builds Obelisk.
+Usage:
+```
+obk-build
+```
+
+## `obk-clean`
+Cleans the Obelisk build folders. If the build is failing unexpectedly, it can be useful to run this command and try again.
+```
+obk-clean
+```
+
 ## State Transitions
 ### `obk-configure`
 Configure all Obelisk nodes.

docs/source/using_obelisk.rst

@@ -311,82 +311,85 @@ Obelisk nodes can be easily configured via a Obelisk configuration (yaml) file.
         # callback_groups:
         publishers:
           - ros_parameter: pub_ctrl_setting
-            # key: pub1
             topic: /obelisk/dummy/ctrl
             msg_type: PositionSetpoint
             history_depth: 10
             callback_group: None
-            non_obelisk: False
         subscribers:
           - ros_parameter: sub_est_setting
-            # key: sub1
             topic: /obelisk/dummy/est
             msg_type: EstimatedState
             history_depth: 10
-            # callback_key: sub_callback1
             callback_group: None
-            non_obelisk: False
         timers:
           - ros_parameter: timer_ctrl_setting
-            # key: timer1
             timer_period_sec: 0.001
             callback_group: None
-            # callback_key: timer_callback1
     estimation:
       - pkg: obelisk_estimation_cpp
         executable: jointencoders_passthrough_estimator
         # callback_groups:
         publishers:
           - ros_parameter: pub_est_setting
-            # key: pub1
             topic: /obelisk/dummy/est
             msg_type: EstimatedState
             history_depth: 10
             callback_group: None
-            non_obelisk: False
         subscribers:
           - ros_parameter: sub_sensor_setting
-            # key: sub1
             topic: /obelisk/dummy/sensor
             msg_type: JointEncoders
             history_depth: 10
-            # callback_key: sub_callback1
             callback_group: None
-            non_obelisk: False
         timers:
           - ros_parameter: timer_est_setting
-            # key: timer1
             timer_period_sec: 0.001
             callback_group: None
-            # callback_key: timer_callback1
     # sensing:
     robot:
       - is_simulated: True
         pkg: obelisk_sim_cpp
         executable: obelisk_mujoco_robot
+        params:
+          ic_keyframe: ic
         # callback_groups:
         # publishers:
         subscribers:
           - ros_parameter: sub_ctrl_setting
-            # key: sub1
             topic: /obelisk/dummy/ctrl
             msg_type: PositionSetpoint
             history_depth: 10
-            # callback_key: sub_callback1
             callback_group: None
-            non_obelisk: False
         sim:
           - ros_parameter: mujoco_setting
             model_xml_path: dummy/dummy.xml
-            n_u: 1
-            time_step: 0.002
             num_steps_per_viz: 5
             sensor_settings:
-            - topic: /obelisk/dummy/sensor
+            - topic: /obelisk/dummy/joint_encoders
               dt: 0.001
-              sensor_type: jointpos
+              msg_type: ObkJointEncoders
               sensor_names:
-              - sensor_joint1
+                joint_pos: jointpos
+                joint_vel: jointvel
+            - topic: /obelisk/dummy/imu
+              dt: 0.002
+              msg_type: ObkImu
+              sensor_names:
+                tip_acc_sensor: accelerometer
+                tip_gyro_sensor: gyro
+                tip_frame_sensor: framequat
+            - topic: /obelisk/dummy/framepose
+              dt: 0.002
+              msg_type: ObkFramePose
+              sensor_names:
+                tip_pos_sensor: framepos
+                tip_orientation_sensor: framequat
+            viz_geoms:
+              dt: 1.0
+              dummy_box: box
+              dummy_box_2: box
+              dummy_sphere: sphere
+
 
 
 Breaking down the configuration file
@@ -412,14 +415,12 @@ First we give the name of this configuration (``dummy``), and which device this
         key: "asdf"
         history_depth: 10
         callback_group: None
-        non_obelisk: False
     subscribers:
       - ros_parameter: sub_est_setting
         topic: /obelisk/dummy/est
         msg_type: EstimatedState
         history_depth: 10
         callback_group: None
-        non_obelisk: False
     timers:
       - ros_parameter: timer_ctrl_setting
         timer_period_sec: 0.001
@@ -436,7 +437,6 @@ Now we need to configure all of the Components in this node. Publishers and subs
 - ``key`` gives the string key associated with the component if not already specified in the code implementation. **Note this is only ever used in the Python implementation. In C++, the key must be specified during component declaration time.**
 - ``history_depth`` (optional) gives the number of messages to hold in the queue before deleting additional messages. If this not set we the use the default value of 10.
 - ``callback_group`` (optional) gives the string name of the callback group to use. The callback groups can be configured within this configuration file. If no value is specified, then the node's default callback group is used.
-- ``non_obelisk`` (optional) determine whether this node can publish non-Obelisk messages. **Note if this is set to true, then this may cause problems with the interfaces, so only use if you are sure this is what you need.** If no value is specified, then the default value of `False` is used.
 
 Timers have the following options.
 
@@ -457,15 +457,12 @@ This is repeated for every non-system node in the block diagram, which in this c
           msg_type: EstimatedState
           history_depth: 10
           callback_group: None
-          non_obelisk: False
       subscribers:
         - ros_parameter: sub_sensor_setting
-          # key: sub1
           topic: /obelisk/dummy/sensor
           msg_type: JointEncoders
           history_depth: 10
           callback_group: None
-          non_obelisk: False
       timers:
         - ros_parameter: timer_est_setting
           timer_period_sec: 0.001
@@ -480,51 +477,71 @@ Lastly, we need to configure the ``robot`` (aka, the system).
     is_simulated: True
     pkg: obelisk_sim_py
     executable: obelisk_mujoco_robot
+    params:
+      ic_keyframe: ic
     # callback_groups:
     # publishers:
     subscribers:
       - ros_parameter: sub_ctrl_setting
-        # key: sub1
         topic: /obelisk/dummy/ctrl
         msg_type: PositionSetpoint
         history_depth: 10
         callback_group: None
-        non_obelisk: False
     sim:
       - ros_parameter: mujoco_setting
         model_xml_path: dummy/dummy.xml
-        n_u: 1
-        time_step: 0.002
         num_steps_per_viz: 5
         sensor_settings:
-        - topic: /obelisk/dummy/sensor
+        - topic: /obelisk/dummy/joint_encoders
           dt: 0.001
-          sensor_type: jointpos
+          msg_type: ObkJointEncoders
           sensor_names:
-          - sensor_joint1
-
+            joint_pos: jointpos
+            joint_vel: jointvel
+        - topic: /obelisk/dummy/imu
+          dt: 0.002
+          msg_type: ObkImu
+          sensor_names:
+            tip_acc_sensor: accelerometer
+            tip_gyro_sensor: gyro
+            tip_frame_sensor: framequat
+        - topic: /obelisk/dummy/framepose
+          dt: 0.002
+          msg_type: ObkFramePose
+          sensor_names:
+            tip_pos_sensor: framepos
+            tip_orientation_sensor: framequat
+          viz_geoms:
+            dt: 1.0
+            dummy_box: box
+            dummy_box_2: box
+            dummy_sphere: sphere
 
 ``is_simulated`` marks if we are running on hardware or in simulation. ``pkg`` and ``executable`` are as before.
+``ic_keyframe`` (optional) in the params section tells the simulation which keyframe to use for an initial condition.
 
 Now, we must configure the Components of the node, which in this example is just a subscriber. These Components have all the same options as the non-system Components given above.
 
 Lastly, since this is a simulation, we must provide the simulator with all relevant information. Here, we are using the Mujoco simulation interface. The new settings here are:
 
-- ``n_u`` gives the number of control inputs (i.e., the number of scalars)
-- ``time_step`` (optional) gives the length of a simulation time step. If no value is provided, the default value of 0.002 seconds will be used.
 - ``num_steps_per_viz`` (optional) gives the number of steps to use between simulation rendering. If no value is provided, the default value of 8 steps will be used.
 
 ``sensor_settings`` is how we can specify what sensors our robot has. Within ``sensor_settings`` we have the following new options:
 
+- ``msg_type`` gives the ROS message type associated with the given group of sensors.
 - ``dt`` gives the sensor publishing period in seconds.
-- ``sensor_type`` gives the Mujoco sensor type. Each sensor type corresponds to a specific Obelisk message type. **Note that the Mujoco XML must have all the sensors listed in the Obelisk configuration file, if you request a sensor here that is not available in Mujoco, there will be an error.** All supported Mujoco sensors and corresponding Obelisk messages are listed below.
+- Each element under ``sensor_names`` follows ``sensor_name: sensor_type`` **Note that the Mujoco XML must have all the sensors listed in the Obelisk configuration file, if you request a sensor here that is not available in Mujoco, there will be an error.** All supported Mujoco sensors and corresponding Obelisk messages are listed below.
+
+=============================================== ====================
+Mujoco sensor type                              Obelisk Message Type
+=============================================== ====================
+``jointpos`` and ``jointvel``                   ``ObkJointEncoders``
+``accelerometer``, ``gyro``, and ``framequat``  ``ObkImu``
+``framepos`` and ``framequat``                  ``ObkFramePose``
+=============================================== ====================
 
-================== ====================
-Mujoco sensor type Obelisk Message Type
-================== ====================
- ``jointpos``        ``JointEncoders``
-================== ====================
+You may have multiple of the same type of sensor in the yaml.
 
-- ``sensor_names`` gives the names of the sensors to query, as given in the Mujoco XML.
+- ``viz_geoms`` (optional) gives a list of visualization geometries that you want the simulation node to publish. The node will read the state of these geoms from the simulator and publish them so an external visualizer can see them. This is designed mostly for visualizing the environment, not the robot.
 
 Thats it! Now we have configured our Obelisk nodes.

obelisk/cpp/obelisk_cpp/CMakeLists.txt

@@ -16,6 +16,9 @@ find_package(obelisk_sensor_msgs REQUIRED)
 find_package(obelisk_estimator_msgs REQUIRED)
 find_package(obelisk_std_msgs REQUIRED)
 
+# --------- Viz Messages --------- #
+find_package(visualization_msgs REQUIRED)
+
 # ------- Mujoco ------- #
 include(FetchContent)
 set(MUJOCO_VERSION "3.1.6" CACHE STRING "mujoco version")
@@ -67,4 +70,5 @@ ament_target_dependencies(ObkCore INTERFACE
   obelisk_control_msgs
   obelisk_estimator_msgs
   obelisk_sensor_msgs
+  visualization_msgs
   std_msgs)