first commit

.gitignore

@@ -0,0 +1,42 @@
+MJDATA.TXT
+MUJOCO_LOG.TXT
+*.slxc
+*.autosave
+*.asv
+slprj/
+html/
+*ert_rtw/
+*_ert_shrlib_rtw/
+*_rtw/
+callgrind.out*
+*.slx.original
+*.zip
+*.tar.gz
+*.gz
+*.o
+references/
+*.exe
+*.out
+*.dylib
+*.dll
+*.so
+*.mexw64
+*.mexa64
+*.mexi64
+*.pdb
+BF_RESULT/
+temp/
+model_report.htmx
+*.pdb
+CP_result/
+BP_result/
+*.r2021b
+*.r2022a
+*.r2021a
+.vs/
+.vscode/
+win64/
+glnxa64/
+maci64/
+maca64/
+arm64/

README.md

@@ -1 +1,122 @@
-# mujoco-simulink-blockset
+# Simulink Blockset for MuJoCo Simulator 
+
+This repository provides a Simulink® C-MEX S-Function block interface to [MuJoCo™ physics engine](https://mujoco.org/).
+
+[![View <File Exchange Title> on File Exchange](https://www.mathworks.com/matlabcentral/images/matlab-file-exchange.svg)](https://www.mathworks.com/matlabcentral/fileexchange/####-file-exchange-title)  
+
+Useful for,
+1. Robot simulation (mobile, biomimetic, grippers, robotic arm)
+2. Development of autonomous algorithms using classical or machine learning based approaches
+3. Camera (RGB, Depth) rendering
+
+## Installation Instructions
+
+### MathWorks&reg; Products (https://www.mathworks.com)
+
+- MATLAB&reg; (required)
+- Simulink&reg; (required)
+- Computer Vision Toolbox&trade; (optional)
+- Robotics System Toolbox&trade; (optional)
+- Control System Toolbox&trade; (optional)
+- Simulink&reg; Coder&trade; (optional)
+
+MATLAB R2022b or newer recommended. Install MATLAB with the above products and then proceed to setting up MuJoCo blocks.
+
+*Note - You may need to rebuild the S-Function if you are using an older release of MATLAB*.
+
+### Simulink Blockset for MuJoCo
+
+#### Windows&reg;:
+
+- Download the latest release of this repository
+- Open MATLAB R2022b. In case you are using a older MATLAB release, please follow the build instructions below to rebuild for a particular MATLAB release
+- Run install.m in MATLAB command window. MuJoCo and GLFW libraries will be downloaded and added to MATLAB Path.
+
+#### Ubuntu&reg;:
+
+- Download the latest release of this repository
+- Download and install GLFW library from Ubuntu terminal
+
+    `sudo apt update && sudo apt install libglfw3 libglfw3-dev`
+- Open MATLAB R2022b. In case you are using a older MATLAB release, please follow the build instructions below to rebuild for a particular MATLAB release
+- Run install.m in MATLAB command window. MuJoCo library will be downloaded and added to MATLAB Path.
+
+## Usage
+Open examples/gettingStarted.slx model and click run 
+
+If the installation is successful, you should see a pendulum model running in a separate window and camera stream displayed by Video Viewer block (Computer Vision Toolbox)
+
+### Blocks
+
+![](mjLib.png){height=250px}
+
+MuJoCo Plant block steps MuJoCo engine, renders visualization window & camera, sets actuator data and outputs sensor readings
+
+It takes an XML (MJCF) as the main block parameter. It auto detects the inputs, sensors, cameras from XML and sets the ports, sample time and other internal configuration.
+
+Inputs can either be a Simulink Bus or vector.
+
+Sensors are output as a Simulink Bus.
+
+RGB and Depth buffers from camera are output as vectors. These can be decoded to Simulink image/matrix using the RGB and Depth Parser blocks.
+
+![](getting_started_clip1.mp4 "Block Usage")
+![](getting_started_clip2.mp4 "Visualization")
+
+## Build Instructions (optional)
+
+Steps for building/rebuilding the C-MEX S-Function code. These instructions are only required if you are cloning the repository instead of downloading the release.
+
+### Windows:
+
+- Install one of the following C++ Compiler
+  - Microsoft&reg; Visual Studio&reg; 2022 or higher (recommended)
+  - (or) [MinGW (GCC 12.2.0 or higher)](https://community.chocolatey.org/packages/mingw)
+- Clone this repository
+- Launch MATLAB and open the repository folder
+    - `>> install`
+- Open tools/ 
+    - Open setupBuild.m. In case you are using MinGW compiler, edit the file and set selectedCompilerWin to "MINGW".
+    - `>> setupBuild`
+    - `>> mex -setup c++`
+    - `>> build`
+
+### Ubuntu
+
+- Install the tools required for compiling the S-Function
+
+    `$ sudo apt update && sudo apt install build-essential git libglfw3 libglfw3-dev `
+- Clone this repository
+
+    `$ git clone <this_repository.git>`
+- Launch MATLAB and open the repository folder. Run the install.m script.
+    - `>> install`
+- Open tools/ and run the following commands in MATLAB command Windows
+    - `>> setupBuild`
+    - `>> mex -setup c++`
+    - `>> build`
+
+
+## License
+
+The license is available in the license.txt file within this repository.
+
+## Acknowledgements
+Cite this work as,
+
+Manoj Velmurugan.  Simulink Blockset for MuJoCo Simulator (https://github.com/mathworks-robotics/mujoco-simulink-blockset), GitHub. Retrieved March 27, 2023. 
+
+
+Refer to [MuJoCo repository](https://github.com/deepmind/mujoco) for guidelines on citing MuJoCo physics engine.
+
+The sample codes and API documentation provided for [MuJoCo](https://mujoco.readthedocs.io/en/latest/overview.html) and [GLFW](https://www.glfw.org/documentation) were used as reference material during development.
+
+MuJoCo and GLFW libraries are dynamically linked against the S-Function and are required for running this blockset.
+
+UR5e MJCF XML from [MuJoCo Menagerie](https://github.com/deepmind/mujoco_menagerie/tree/main/universal_robots_ur5e) was used for creating demo videos.
+
+## Community Support
+
+You can post your queries on the [MATLAB&reg; Central&trade; File Exchange](https://www.mathworks.com/matlabcentral/fileexchange/####-file-exchange-title) page.
+
+Copyright 2023 The MathWorks, Inc.

SECURITY.md

@@ -0,0 +1,6 @@
+# Reporting Security Vulnerabilities 
+
+If you believe you have discovered a security vulnerability, please report it to 
+[security@mathworks.com](mailto:security@mathworks.com). Please see 
+[MathWorks Vulnerability Disclosure Policy for Security Researchers](https://www.mathworks.com/company/aboutus/policies_statements/vulnerability-disclosure-policy.html) 
+for additional information.  

blocks/dummy.xml

@@ -0,0 +1,55 @@
+ <mujoco model="arm_with_cam">
+    <option timestep="0.005" iterations="50" solver="Newton" tolerance="1e-10"/>
+    <visual>
+        <rgba haze=".3 .3 .3 1"/>
+    </visual>
+    <asset>
+        <texture name="texplane" type="2d" builtin="checker" rgb1=".25 .25 .25" rgb2=".3 .3 .3" width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
+        <material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
+    </asset>
+
+    <sensor>
+        <framequat name="imu" objtype="body" objname="armBody" />
+    </sensor>
+
+    <worldbody>
+        <geom name="floor" pos="0 0 0" size="0 0 1" type="plane" material="matplane"/>
+        <light directional="true" diffuse=".7 .7 .8" specular=".2 .2 .2" pos="0 0 1" dir="0 0 -1"/>
+        <camera name="PerspectiveCamera" mode="fixed" pos="0 0 6" euler="0 -20 90"/>
+
+        <body pos="0 0 0">
+            <geom name="base1" type="box" size="0.1 0.1 1"  pos="0 0 1" rgba="0.2 0.6 0.5 1"/>
+
+            <body name="armBody" pos="0.25 0 2">
+                <geom name="arm" type="box" size="0.1 0.8 0.1" pos="0 0.8 0" rgba="0.2 0.6 0.5 1"/>
+                <!-- <joint name="joint" type="hinge" pos="0 0 0" axis="1 0 0" frictionloss="50"/> -->
+                <joint name="joint" type="hinge" pos="0 0 0" axis="1 0 0" frictionloss="0"/>
+                <camera name="camera" mode="fixed" pos="0.2 0 0" euler="0 -90 0"/>
+                <geom name="cameraGeom" type="box" pos="0.15 0 0" size="0.05 0.1 0.1" rgba="0.4 0.2 0.6 1"/>
+            </body>
+        </body>
+
+        <body pos="0 0 0">
+            <geom name="red box" type="box" size="0.5 0.5 0.5"  pos="4 0 1" rgba="1 0 0 1"/>
+            <freejoint/>
+        </body>
+
+        <body pos="0 0 0">
+            <geom name="blue box" type="box" size="0.5 0.5 0.5"  pos="4 0 1.5" rgba="0 1 0 1"/>
+            <freejoint/>
+        </body>
+
+        <body pos="0 0 0">
+            <geom name="green box" type="box" size="0.5 0.5 0.5"  pos="4 0 2" rgba="0 0 1 1"/>
+            <freejoint/>
+        </body>
+
+
+    </worldbody>
+
+    <actuator>
+        <motor name="armMotor" joint="joint" gear="100" />
+    </actuator>
+
+    <!-- Copyright 2022-2023 The MathWorks, Inc. -->
+</mujoco>

blocks/mj_busExist.m

@@ -0,0 +1,4 @@
+function x = mj_busExist(busName)
+% Copyright 2022-2023 The MathWorks, Inc.
+    x=evalin('base', strcat("exist('", busName, "', 'var')"));
+end

blocks/mj_initbus.m

@@ -0,0 +1,12 @@
+function [a,b,c,d,e] = mj_initbus(xmlPath)
+% Copyright 2022-2023 The MathWorks, Inc.
+% Lets just be on safe side and run mj_initbus_mex in a separate
+% process. It calls glfw functions which work best in a main thread of
+% a separate process.
+persistent mh
+if ~(isa(mh,'matlab.mex.MexHost') && isvalid(mh))
+    mh = mexhost;
+end
+[a,b,c,d, e] = feval(mh, 'mj_initbus_mex', xmlPath);
+%     [a,b,c,d,e] = mj_initbus_mex(xmlPath);
+end

blocks/mj_maskinit.m

@@ -0,0 +1,139 @@
+% Copyright 2022-2023 The MathWorks, Inc.
+mjBlk = gcb;
+mo = get_param(mjBlk,'MaskObject');
+%% Sensor Bus Config
+blankBusPath = [mjBlk, '/sensorToBus/blankSensorBus'];
+sensorToBusPath = [mjBlk, '/sensorToBus'];
+sensorOut1Path = [mjBlk, '/sensor'];
+portIndex = 1;
+try
+    sensorFieldnames = fieldnames(Simulink.Bus.createMATLABStruct(sensorBus));
+    %fails when it is an empty bus TODO. remove try catch
+catch
+    sensorFieldnames = [];
+end
+
+if isempty(sensorFieldnames)
+    mo.getDialogControl('sensorBusText').Prompt = ['Sensor Bus Type: ', 'NA'];
+    set_param(sensorToBusPath, 'Commented', 'through');
+    replacer(mjBlk, 'sensor', 'simulink/Sinks/Terminator')
+else
+    mo.getDialogControl('sensorBusText').Prompt = ['Sensor Bus Type: ', sensorBus];
+    set_param(sensorToBusPath, 'Commented', 'off');
+    outportName = 'sensor';
+    replacer(mjBlk, outportName, 'simulink/Sinks/Out1');
+    set_param([mjBlk, '/', outportName], "Port", num2str(portIndex));
+    portIndex = portIndex+1;
+
+    set_param(blankBusPath, 'OutDataTypeStr', ['Bus: ', sensorBus]);
+    set_param([sensorToBusPath '/parser'], 'OutputBusName', sensorBus);
+end
+
+%% Control Bus Config
+controlInportPath = [mjBlk, '/', 'u'];
+uToVectorPath = [mjBlk, '/', 'uToVector'];
+uPortExpander = [mjBlk, '/uPortExpander'];
+try
+    controlFieldnames = fieldnames(Simulink.Bus.createMATLABStruct(controlBus));
+    %fails when controlBus is an empty bus TODO. remove try catch
+catch
+    controlFieldnames = [];
+end
+
+% Remove inport for passive simulation case
+if isempty(controlFieldnames)
+    replacer(mjBlk, 'u', 'simulink/Sources/Ground');
+    set_param(uToVectorPath, 'Commented', 'through');
+    mo.getDialogControl('controlBusText').Prompt = 'Control Vector Type: NA';
+    set_param(uPortExpander, 'Commented', 'through');
+else
+    set_param(uPortExpander, 'Commented', 'off');
+    replacer(mjBlk, 'u', 'simulink/Sources/In1');
+    % Set bus or vector
+    if strcmp(get_param(mjBlk, 'controlInterfaceType'), 'Bus')
+        set_param(controlInportPath, 'OutDataTypeStr', ['Bus: ', controlBus]);
+        set_param(uToVectorPath, 'Commented', 'off');
+        mo.getDialogControl('controlBusText').Prompt = ['Control Bus Type: ', controlBus];
+        set_param(controlInportPath, 'PortDimensions', '-1')
+    elseif strcmp(get_param(mjBlk, 'controlInterfaceType'), 'Vector')
+        set_param(controlInportPath, 'OutDataTypeStr', 'Inherit: auto');
+        set_param(uToVectorPath, 'Commented', 'through');
+        mo.getDialogControl('controlBusText').Prompt = ['Control Vector Type: [', strjoin(controlFieldnames, ', '), ']'];
+        inputDim = [length(controlFieldnames) 1];
+        set_param(controlInportPath, 'PortDimensions', ['[' num2str(inputDim), ']']);
+    end
+end
+
+%% RGB
+
+% blankBusPath = [mjBlk, '/rgbToBus/blankBus'];
+% rgbToBusPath = [mjBlk, '/rgbToBus'];
+rgbOut1Path = [mjBlk, '/rgb'];
+try
+    rgbFieldnames = fieldnames(Simulink.Bus.createMATLABStruct(rgbBus));
+    %fails when it is an empty bus TODO. remove try catch
+    mo.getDialogControl('rgbBusText').Prompt = ['RGB Bus Type: ', rgbBus];
+catch
+    rgbFieldnames = [];
+    mo.getDialogControl('rgbBusText').Prompt = ['RGB Bus Type: ', 'NA'];
+end
+
+if isempty(rgbFieldnames)
+    replacer(mjBlk, 'rgb', 'simulink/Sinks/Terminator')
+else
+    outportName = 'rgb';
+    replacer(mjBlk, outportName, 'simulink/Sinks/Out1');
+    set_param([mjBlk, '/', outportName], "Port", num2str(portIndex));
+    portIndex = portIndex+1;
+end
+
+%% Depth
+mo.getDialogControl('rgbBusText').Prompt = ['RGB Bus Type: ', rgbBus];
+depthOut1Path = [mjBlk, '/depth'];
+depthConverterPath = [mjBlk, '/OpenGL Depth conversion'];
+try
+    depthFieldnames = fieldnames(Simulink.Bus.createMATLABStruct(depthBus));
+    %fails when it is an empty bus TODO. remove try catch
+    mo.getDialogControl('depthBusText').Prompt = ['Depth Bus Type: ', depthBus];
+catch
+    depthFieldnames = [];
+    mo.getDialogControl('depthBusText').Prompt = ['Depth Bus Type: ', 'NA'];
+end
+depthOutputOption = strcmp(get_param(mjBlk, 'depthOutOption'), 'on');
+
+if isempty(depthFieldnames) || ~depthOutputOption
+    set_param(depthConverterPath, 'Commented', 'on');
+    replacer(mjBlk, 'depth', 'simulink/Sinks/Terminator')
+else
+    set_param(depthConverterPath, 'Commented', 'off');
+    outportName = 'depth';
+    replacer(mjBlk, 'depth', 'simulink/Sinks/Out1');
+    set_param([mjBlk, '/', outportName], "Port", num2str(portIndex));
+    portIndex = portIndex+1;
+end
+
+[znear, zfar] = mj_depth_near_far(xmlFile);
+set_param(mjBlk, 'znear', num2str(znear));
+set_param(mjBlk, 'zfar', num2str(zfar));
+
+%% Sample Time
+sampleTime = mj_sampletime(xmlFile);
+set_param(mjBlk, 'sampleTime', num2str(sampleTime));
+
+mo.getDialogControl('sampleTimeText').Prompt = ['Sample Time: ', num2str(sampleTime)];
+
+function replacer(blk, oldname, newtype)
+    oldpath = [blk, '/', oldname];
+    newTypeWithoutLib = strsplit(newtype, '/');
+    newTypeWithoutLib = newTypeWithoutLib{end};
+    if strcmp(newTypeWithoutLib, 'In1')
+        newTypeWithoutLib = 'Inport';
+    elseif strcmp(newTypeWithoutLib, 'Out1')
+        newTypeWithoutLib = 'Outport';
+    end
+    if ~strcmp(get_param(oldpath, 'BlockType'), newTypeWithoutLib)
+        position =  get_param(oldpath, 'Position');
+        delete_block(oldpath);
+        add_block(newtype, oldpath, 'Position', position);
+    end
+end