diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h
index b7a1e635..03585e6c 100644
--- a/include/mujoco/mjdata.h
+++ b/include/mujoco/mjdata.h
@@ -529,7 +529,7 @@ typedef mjtNum (*mjfTime)(void);
 typedef mjtNum (*mjfAct)(const mjModel* m, const mjData* d, int id);
 
 // collision detection
-typedef int (*mjfCollision)(const mjModel* m, const mjData* d,
-                            mjContact* con, int g1, int g2, mjtNum margin);
+typedef int (*mjfCollision)(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                            mjtNum margin);
 
 #endif  // MUJOCO_MJDATA_H_
diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c
index 5b10f821..e30b7e16 100644
--- a/src/engine/engine_collision_box.c
+++ b/src/engine/engine_collision_box.c
@@ -94,9 +94,7 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin,
 
 
 // sphere : box
-int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con,
-                  int g1, int g2, mjtNum margin)
-{
+int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO;
 
   return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
@@ -593,9 +591,7 @@ skip:
 
 
 // capsule : box
-int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
-                   int g1, int g2, mjtNum margin)
-{
+int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
   return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
@@ -1342,7 +1338,7 @@ edgeedge:
 
 
 // box : box
-int mjc_BoxBox(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   int num = _boxbox(m, d, con, g1, g2, margin);
 
   // use dim field to mark: -1: bad, 0: good
diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c
index d25a6401..a232ba1f 100644
--- a/src/engine/engine_collision_convex.c
+++ b/src/engine/engine_collision_convex.c
@@ -868,8 +868,7 @@ static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) {
 
 
 // multi-point convex-convex collision, using libccd
-int mjc_Convex(const mjModel* m, const mjData* d,
-               mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   // init ccd objects
   mjCCDObj obj1, obj2;
   mjc_initCCDObj(&obj1, m, d, g1, margin);
@@ -998,8 +997,7 @@ static int addplanemesh(mjContact* con, const float vertex[3],
 
 
 // plane-convex collision, using libccd
-int mjc_PlaneConvex(const mjModel* m, const mjData* d,
-                    mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
   mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
   ccd_vec3_t dir, vec;
@@ -1136,8 +1134,7 @@ static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, m
 
 
 // entry point for heightfield collisions
-int mjc_ConvexHField(const mjModel* m, const mjData* d,
-                     mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   // hfield frame
   const mjtNum* pos1 = d->geom_xpos + 3*g1;
   const mjtNum* mat1 = d->geom_xmat + 9*g1;
@@ -1565,8 +1562,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
 //----------------------------  flex collisions ---------------------------------------------
 
 // geom-elem or elem-elem or vert-elem convex collision using ccd
-int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
-                   int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
+int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1,
+                   int f2, int e2, mjtNum margin) {
   mjCCDObj obj1, obj2;
   mjc_initCCDObj(&obj1, m, d, g1, margin);
   mjc_initCCDObj(&obj2, m, d, -1, margin);
@@ -1595,8 +1592,8 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
 
 
 // test a height field and a flex element for collision
-int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
-                   int g, int f, int e, mjtNum margin) {
+int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, int e,
+                   mjtNum margin) {
   mjtNum vec[3], dx, dy;
   mjtNum xmin, xmax, ymin, ymax, zmin, zmax;
   int dr[2], cnt, rmin, rmax, cmin, cmax;
diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h
index 3a27fb19..4f94b80c 100644
--- a/src/engine/engine_collision_convex.h
+++ b/src/engine/engine_collision_convex.h
@@ -79,20 +79,17 @@ void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
 void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
 
 // pairwise geom collision functions using ccd
-int mjc_PlaneConvex(const mjModel* m, const mjData* d,
-                    mjContact* con, int g1, int g2, mjtNum margin);
-int mjc_ConvexHField(const mjModel* m, const mjData* d,
-                     mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_Convex(const mjModel* m, const mjData* d,
-                     mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+MJAPI int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
 
 // geom-elem or elem-elem or vert-elem collision function using ccd
-int mjc_ConvexElem    (const mjModel* m, const mjData* d, mjContact* con,
-                       int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin);
+int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1,
+                   int f2, int e2, mjtNum margin);
 
 // heightfield-elem collision function using ccd
-int mjc_HFieldElem    (const mjModel* m, const mjData* d, mjContact* con,
-                       int g, int f, int e, mjtNum margin);
+int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, int e,
+                   mjtNum margin);
 
 // fix contact frame normal
 void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2);
diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c
index 4ce09120..145dfc50 100644
--- a/src/engine/engine_collision_primitive.c
+++ b/src/engine/engine_collision_primitive.c
@@ -51,16 +51,14 @@ static int mjraw_PlaneSphere(mjContact* con, mjtNum margin,
 
 
 // plane : sphere
-int mjc_PlaneSphere(const mjModel* m, const mjData* d,
-                    mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
   return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
 
 
 // plane : capsule
-int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
-                     mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
 
   // get capsule axis, segment = scaled axis
@@ -89,8 +87,7 @@ int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
 
 
 // plane : cylinder
-int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
-                      mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
   mjtNum normal[3] = {mat1[2], mat1[5], mat1[8]};
   mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
@@ -193,8 +190,7 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
 
 
 // plane : box
-int mjc_PlaneBox(const mjModel* m, const mjData* d,
-                 mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
 
   // get normal, difference between centers, normal distance
@@ -277,8 +273,7 @@ static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
 
 
 // sphere : sphere
-int mjc_SphereSphere(const mjModel* m, const mjData* d,
-                     mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
   return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
@@ -304,16 +299,15 @@ int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
 
 
 // sphere : capsule
-int mjc_SphereCapsule(const mjModel* m, const mjData* d,
-                      mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
   return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
 
 
 // sphere : cylinder
-int mjc_SphereCylinder(const mjModel* m, const mjData* d,
-                       mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                       mjtNum margin) {
   mjGETINFO
 
   // get cylinder sizes and axis
@@ -482,8 +476,8 @@ int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
 
 
 // capsule : capsule
-int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
-                       mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                       mjtNum margin) {
   mjGETINFO
   return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
 }
diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h
index 5fc675e6..806e8742 100644
--- a/src/engine/engine_collision_primitive.h
+++ b/src/engine/engine_collision_primitive.h
@@ -55,32 +55,32 @@ int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos,
                           mjtNum rt);
 
 // plane collisions
-MJAPI int mjc_PlaneSphere     (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_PlaneCapsule    (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_PlaneCylinder   (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_PlaneBox        (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
+MJAPI int mjc_PlaneSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                          mjtNum margin);
+MJAPI int mjc_PlaneCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                           mjtNum margin);
+MJAPI int mjc_PlaneCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                            mjtNum margin);
+MJAPI int mjc_PlaneBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                       mjtNum margin);
 
 // sphere and capsule collisions
-MJAPI int mjc_SphereSphere    (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_SphereCapsule   (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_SphereCylinder  (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_CapsuleCapsule  (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
+MJAPI int mjc_SphereSphere(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                           mjtNum margin);
+MJAPI int mjc_SphereCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                            mjtNum margin);
+MJAPI int mjc_SphereCylinder(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                             mjtNum margin);
+MJAPI int mjc_CapsuleCapsule(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                             mjtNum margin);
 
 // box collisions: from engine_collision_box.c
-MJAPI int mjc_CapsuleBox      (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_SphereBox       (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
-MJAPI int mjc_BoxBox          (const mjModel* m, const mjData* d,
-                               mjContact* con, int g1, int g2, mjtNum margin);
+MJAPI int mjc_CapsuleBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                         mjtNum margin);
+MJAPI int mjc_SphereBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                        mjtNum margin);
+MJAPI int mjc_BoxBox(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
+                     mjtNum margin);
 
 #ifdef __cplusplus
 }
diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c
index cd959411..cd074841 100644
--- a/src/engine/engine_collision_sdf.c
+++ b/src/engine/engine_collision_sdf.c
@@ -545,7 +545,7 @@ static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
 //             1 = normal points OUT of SDF (for flex-SDF where SDF is g1)
 static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
                       const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist,
-                      int cnt, const mjModel* m, const mjSDF* s, mjData* d,
+                      int cnt, const mjModel* m, const mjSDF* s, const mjData* d,
                       int flipNormal) {
   // check if there is a collision
   if (dist > 0 || isknown(points, x, cnt)) {
@@ -584,7 +584,7 @@ static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3],
 
 // finds minimum of Frank-Wolfe objective
 static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
-                             const mjModel* m, const mjSDF* sdf, mjData* d) {
+                             const mjModel* m, const mjSDF* sdf, const mjData* d) {
   for (int step=0; step < m->opt.sdf_iterations; step++) {
     mjtNum best = mjMAXVAL, fun, s[3], grad[3];
 
@@ -614,7 +614,7 @@ static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners,
 
 // finds minimum using gradient descent
 static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
-                           mjData* d, int niter) {
+                           const mjData* d, int niter) {
   const mjtNum c = .1;       // reduction factor for the target decrease in the objective function
   const mjtNum rho = .5;     // reduction factor for the gradient scaling (alpha)
   const mjtNum amin = 1e-4;  // minimum value for alpha
@@ -664,7 +664,7 @@ static mjtNum stepGradient(mjtNum x[3], const mjModel* m, const mjSDF* s,
 
 // stricter triangle collision
 static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
-                             const mjSDF* sdf, mjData* d) {
+                             const mjSDF* sdf, const mjData* d) {
   mjtNum edges[6];
   mjtNum normal[3], center[3];
   mjtNum v[9], cross[9], p[3];
@@ -737,7 +737,7 @@ static int triangleIntersect(const mjtNum triangle[9], const mjModel* m,
 // intersect with circumsphere of bounding box
 static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3],
                         const mjtNum rotation[9], const mjModel* m,
-                        const mjSDF* s, mjData* d) {
+                        const mjSDF* s, const mjData* d) {
   mjtNum candidate[3];
   mjtNum r = mju_norm3(bvh+3);
 
@@ -806,7 +806,7 @@ static int selectFPS(const mjtNum* candidate, const mjtNum* dist, int ncandidate
 // Process triangle corners against SDF using Halton sampling + Frank-Wolfe.
 // Corners are assumed to already be in SDF local coordinates.
 // Adds penetrating candidates to candidate/dist arrays.
-static void processSdfCorners(const mjtNum corners[9], const mjModel* m, mjData* d,
+static void processSdfCorners(const mjtNum corners[9], const mjModel* m, const mjData* d,
                               const mjSDF* sdf, int nstartpts,
                               mjtNum* candidate, mjtNum* dist, int* ncandidate) {
   mjtNum x[3], depth;
@@ -851,7 +851,7 @@ static void processSdfCorners(const mjtNum corners[9], const mjModel* m, mjData*
 // Context for inline face processing during BVH traversal
 typedef struct {
   const mjModel* m;
-  mjData* d;
+  const mjData* d;
   const mjSDF* sdf;
   const mjtNum* offset;
   const mjtNum* rotation;
@@ -903,7 +903,7 @@ typedef int (*BVHLeafCallback)(int leaf_id, int node, void* ctx);
 // ctx: user context passed to callback
 static void traverseBVH(const mjtNum* bvh, const int* nodeid, const int* child,
                         mjtByte* bvh_active, const mjtNum* offset, const mjtNum* rotation,
-                        const mjModel* m, mjData* d, const mjSDF* sdf,
+                        const mjModel* m, const mjData* d, const mjSDF* sdf,
                         BVHLeafCallback callback, void* ctx) {
   int stack[64];
   int nstack = 0;
@@ -952,14 +952,14 @@ static int meshFaceCallback(int face_id, int node, void* ctx) {
 //------------------------------ collision functions -----------------------------------------------
 
 // collision between a height field and a signed distance field
-int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_HFieldSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mju_warning("HField vs SDF collision not yet supported!");
   return 0;
 }
 
 
 // collision between a mesh and a signed distance field
-int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO;
 
   mjtNum offset[3], rotation[9];
@@ -988,7 +988,7 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
   // set up context for inline BVH processing
   MeshSDFContext ctx;
   ctx.m = m;
-  ctx.d = (mjData*)d;
+  ctx.d = d;
   ctx.sdf = &sdf;
   ctx.offset = offset;
   ctx.rotation = rotation;
@@ -1008,14 +1008,14 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
     mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL;
 
     traverseBVH(bvh, nodeid, child, bvh_active, ctx.offset, ctx.rotation,
-                m, (mjData*)d, ctx.sdf, meshFaceCallback, &ctx);
+                m, d, ctx.sdf, meshFaceCallback, &ctx);
   }
 
   // if few candidates, add them all directly
   if (ncandidate <= mjMAXCONPAIR) {
     for (int i = 0; i < ncandidate; i++) {
       cnt = addContact(points, con, candidate + 3*i, pos2, sdf_quat,
-                       dist[i], cnt, m, &sdf, (mjData*)d, 0);
+                       dist[i], cnt, m, &sdf, d, 0);
     }
     return cnt;
   }
@@ -1028,14 +1028,14 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g
   for (int i = 0; i < nselected; i++) {
     int idx = selected_indices[i];
     cnt = addContact(points, con, candidate + 3*idx, pos2, sdf_quat,
-                     dist[idx], cnt, m, &sdf, (mjData*)d, 0);
+                     dist[idx], cnt, m, &sdf, d, 0);
   }
 
   return cnt;
 }
 
 // collision between two SDFs
-int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
+int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO;
   size1 = m->geom_aabb + 6*g1;
   size2 = m->geom_aabb + 6*g2;
@@ -1148,20 +1148,20 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
 
     // start counters
     if (sdf_ptr[0]) {
-      sdf_ptr[0]->compute(m, (mjData*)d, instance[0], mjPLUGIN_SDF);
+      sdf_ptr[0]->compute(m, d, instance[0], mjPLUGIN_SDF);
     }
 
     // gradient descent - we use a special function of the two SDF as objective
     sdf.type = mjSDFTYPE_COLLISION;
-    dist = stepGradient(x, m, &sdf, (mjData*)d, m->opt.sdf_iterations);
+    dist = stepGradient(x, m, &sdf, d, m->opt.sdf_iterations);
 
     // inexact SDFs can yield spurious collisions, filter them by projecting on the midsurface
     sdf.type = mjSDFTYPE_INTERSECTION;
-    dist = stepGradient(x, m, &sdf, (mjData*)d, 1);
+    dist = stepGradient(x, m, &sdf, d, 1);
 
     // contact point and normal - we use the midsurface where SDF1=SDF2 as zero level set
     sdf.type = mjSDFTYPE_MIDSURFACE;
-    cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, (mjData*)d, 0);
+    cnt = addContact(contacts, con, x, pos2, quat2, dist, cnt, m, &sdf, d, 0);
 
     // SHOULD NOT OCCUR
     if (cnt > mjMAXCONPAIR) {
@@ -1175,7 +1175,7 @@ int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, m
 // Context for flex element processing during BVH traversal
 typedef struct {
   const mjModel* m;
-  mjData* d;
+  const mjData* d;
   const mjSDF* sdf;
   const mjtNum* offset;
   const mjtNum* rotation;
@@ -1240,7 +1240,7 @@ static int flexElemCallback(int elem_idx, int node, void* ctx) {
 }
 
 
-int mjc_FlexSDF(const mjModel* m, mjData* d, mjContact* con,
+int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
                 int g, int f, mjtNum margin) {
   // g = SDF geom, f = flex
   int dim = m->flex_dim[f];
diff --git a/src/engine/engine_collision_sdf.h b/src/engine/engine_collision_sdf.h
index cf572450..b50f3082 100644
--- a/src/engine/engine_collision_sdf.h
+++ b/src/engine/engine_collision_sdf.h
@@ -36,16 +36,16 @@ MJAPI void mjc_gradient(const mjModel* m, const mjData* d, const mjSDF* s, mjtNu
                         const mjtNum x[3]);
 
 // collision between a height field and a signed distance field
-int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_HFieldSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
 
 // collision between a mesh and a signed distance field
-int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_MeshSDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
 
 // collision between two signed distance fields
-int mjc_SDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
+int mjc_SDF(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtNum margin);
 
 // collision between entire flex and SDF geom (batched processing)
-int mjc_FlexSDF(const mjModel* m, mjData* d, mjContact* con,
+int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con,
                 int g, int f, mjtNum margin);
 
 #ifdef __cplusplus