diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 7e2befa0..205b7762 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -357,6 +357,26 @@ last argument to :ref:`mj_local2global`. .. mujoco-include:: mjtSameFrame +.. _mjtFlexSelf: + +mjtFlexSelf +~~~~~~~~~~~~ + +Types of flex self-collisions midphase. + +.. mujoco-include:: mjtFlexSelf + + +.. _mjtSDFType: + +mjtSDFType +~~~~~~~~~~~ + +Formulas used to combine SDFs when calling mjc_distance and mjc_gradient. + +.. mujoco-include:: mjtSDFType + + .. _tyDataEnums: Data diff --git a/doc/includes/references.h b/doc/includes/references.h index d8071b2c..363906a0 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -766,6 +766,12 @@ typedef enum mjtFlexSelf_ { // mode for flex selfcollide mjFLEXSELF_SAP, // use SAP in midphase mjFLEXSELF_AUTO // choose between BVH and SAP automatically } mjtFlexSelf; +typedef enum mjtSDFType_ { // signed distance function (SDF) type + mjSDFTYPE_SINGLE = 0, // single SDF + mjSDFTYPE_INTERSECTION, // max(A, B) + mjSDFTYPE_MIDSURFACE, // A - B + mjSDFTYPE_COLLISION, // A + B + abs(max(A, B)) +} mjtSDFType; struct mjLROpt_ { // options for mj_setLengthRange() // flags int mode; // which actuators to process (mjtLRMode) @@ -1586,6 +1592,15 @@ struct mjpPlugin_ { void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes); }; typedef struct mjpPlugin_ mjpPlugin; +struct mjSDF_ { + const mjpPlugin** plugin; + int* id; + mjtSDFType type; + mjtNum* relpos; + mjtNum* relmat; + mjtGeom* geomtype; +}; +typedef struct mjSDF_ mjSDF; typedef enum mjtGridPos_ { // grid position for overlay mjGRID_TOPLEFT = 0, // top left mjGRID_TOPRIGHT, // top right @@ -3637,6 +3652,10 @@ void mju_insertionSortInt(int* list, int n); mjtNum mju_Halton(int index, int base); char* mju_strncpy(char *dst, const char *src, int n); mjtNum mju_sigmoid(mjtNum x); +const mjpPlugin* mjc_getSDF(const mjModel* m, int id); +mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]); +void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNum gradient[3], + const mjtNum x[3]); void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, mjtNum* A, mjtNum* B, mjtNum* C, mjtNum* D); void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuation, diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index b6bdbfc6..ceaa76d1 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -407,6 +407,14 @@ typedef enum mjtFlexSelf_ { // mode for flex selfcollide } mjtFlexSelf; +typedef enum mjtSDFType_ { // signed distance function (SDF) type + mjSDFTYPE_SINGLE = 0, // single SDF + mjSDFTYPE_INTERSECTION, // max(A, B) + mjSDFTYPE_MIDSURFACE, // A - B + mjSDFTYPE_COLLISION, // A + B + abs(max(A, B)) +} mjtSDFType; + + //---------------------------------- mjLROpt ------------------------------------------------------- struct mjLROpt_ { // options for mj_setLengthRange() diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index 0fc31a6c..44f2af20 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -135,6 +135,16 @@ struct mjpPlugin_ { }; typedef struct mjpPlugin_ mjpPlugin; +struct mjSDF_ { + const mjpPlugin** plugin; + int* id; + mjtSDFType type; + mjtNum* relpos; + mjtNum* relmat; + mjtGeom* geomtype; +}; +typedef struct mjSDF_ mjSDF; + #if defined(__has_attribute) #if __has_attribute(constructor) diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index b955cb13..784d1ab8 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1308,6 +1308,19 @@ MJAPI char* mju_strncpy(char *dst, const char *src, int n); MJAPI mjtNum mju_sigmoid(mjtNum x); +//---------------------------------- Signed Distance Function -------------------------------------- + +// get sdf from geom id +MJAPI const mjpPlugin* mjc_getSDF(const mjModel* m, int id); + +// signed distance function +MJAPI mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]); + +// gradient of sdf +MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNum gradient[3], + const mjtNum x[3]); + + //---------------------------------- Derivatives --------------------------------------------------- // Finite differenced transition matrices (control theory notation) diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index dd1a1fad..a1cab44f 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -428,6 +428,17 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjFLEXSELF_AUTO', 4), ]), )), + ('mjtSDFType', + EnumDecl( + name='mjtSDFType', + declname='enum mjtSDFType_', + values=dict([ + ('mjSDFTYPE_SINGLE', 0), + ('mjSDFTYPE_INTERSECTION', 1), + ('mjSDFTYPE_MIDSURFACE', 2), + ('mjSDFTYPE_COLLISION', 3), + ]), + )), ('mjtTaskStatus', EnumDecl( name='mjtTaskStatus', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index f62a0e7a..ef8bcd58 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -8570,6 +8570,99 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Sigmoid function over 0<=x<=1 using quintic polynomial.', )), + ('mjc_getSDF', + FunctionDecl( + name='mjc_getSDF', + return_type=PointerType( + inner_type=ValueType(name='mjpPlugin', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='id', + type=ValueType(name='int'), + ), + ), + doc='get sdf from geom id', + )), + ('mjc_distance', + FunctionDecl( + name='mjc_distance', + return_type=ValueType(name='mjtNum'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData', is_const=True), + ), + ), + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjSDF', is_const=True), + ), + ), + FunctionParameterDecl( + name='x', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(3,), + ), + ), + ), + doc='signed distance function', + )), + ('mjc_gradient', + FunctionDecl( + name='mjc_gradient', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData', is_const=True), + ), + ), + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjSDF', is_const=True), + ), + ), + FunctionParameterDecl( + name='gradient', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='x', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(3,), + ), + ), + ), + doc='gradient of sdf', + )), ('mjd_transitionFD', FunctionDecl( name='mjd_transitionFD', diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index e3ec1bc8..5ccdc9b8 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -8966,6 +8966,55 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), ), )), + ('mjSDF', + StructDecl( + name='mjSDF', + declname='struct mjSDF_', + fields=( + StructFieldDecl( + name='plugin', + type=PointerType( + inner_type=PointerType( + inner_type=ValueType(name='mjpPlugin', is_const=True), + ), + ), + doc='', + ), + StructFieldDecl( + name='id', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='', + ), + StructFieldDecl( + name='type', + type=ValueType(name='mjtSDFType'), + doc='', + ), + StructFieldDecl( + name='relpos', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='', + ), + StructFieldDecl( + name='relmat', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + doc='', + ), + StructFieldDecl( + name='geomtype', + type=PointerType( + inner_type=ValueType(name='mjtGeom'), + ), + doc='', + ), + ), + )), ('mjrRect', StructDecl( name='mjrRect', diff --git a/src/engine/engine_collision_sdf.h b/src/engine/engine_collision_sdf.h index c6acbe1c..c0ea7cdb 100644 --- a/src/engine/engine_collision_sdf.h +++ b/src/engine/engine_collision_sdf.h @@ -19,27 +19,14 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { #endif -typedef enum mjtSDFType_ { // signed distance function (SDF) type - mjSDFTYPE_SINGLE = 0, // single SDF - mjSDFTYPE_INTERSECTION, // max(A, B) - mjSDFTYPE_MIDSURFACE, // A - B - mjSDFTYPE_COLLISION, // A + B + abs(max(A, B)) -} mjtSDFType; - -struct mjSDF_ { - const mjpPlugin** plugin; - int* id; - mjtSDFType type; - mjtNum* relpos; - mjtNum* relmat; - mjtGeom* geomtype; -}; -typedef struct mjSDF_ mjSDF; +// get sdf from geom id +MJAPI const mjpPlugin* mjc_getSDF(const mjModel* m, int id); // signed distance function MJAPI mjtNum mjc_distance(const mjModel* m, const mjData* d, const mjSDF* s, const mjtNum x[3]); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index b7137723..c1042de6 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -419,6 +419,12 @@ public enum mjtFlexSelf : int{ mjFLEXSELF_SAP = 3, mjFLEXSELF_AUTO = 4, } +public enum mjtSDFType : int{ + mjSDFTYPE_SINGLE = 0, + mjSDFTYPE_INTERSECTION = 1, + mjSDFTYPE_MIDSURFACE = 2, + mjSDFTYPE_COLLISION = 3, +} public enum mjtPluginCapabilityBit : int{ mjPLUGIN_ACTUATOR = 1, mjPLUGIN_SENSOR = 2,