diff --git a/genesis/engine/couplers/sap_coupler.py b/genesis/engine/couplers/sap_coupler.py
index 32aa5d7d76..7e256872bb 100644
--- a/genesis/engine/couplers/sap_coupler.py
+++ b/genesis/engine/couplers/sap_coupler.py
@@ -498,12 +498,12 @@ def compute_fem_surface_tet_aabb(self, i_step: ti.i32):
         aabbs = ti.static(self.fem_surface_tet_aabb.aabbs)
         for i_b, i_se in ti.ndrange(self.fem_solver._B, self.fem_solver.n_surface_elements):
             i_e = self.fem_solver.surface_elements[i_se]
-            i_v = self.fem_solver.elements_i[i_e].el2v
+            i_vs = self.fem_solver.elements_i[i_e].el2v
 
             aabbs[i_b, i_se].min.fill(np.inf)
             aabbs[i_b, i_se].max.fill(-np.inf)
             for i in ti.static(range(4)):
-                pos_v = self.fem_solver.elements_v[i_step, i_v[i], i_b].pos
+                pos_v = self.fem_solver.elements_v[i_step, i_vs[i], i_b].pos
                 aabbs[i_b, i_se].min = ti.min(aabbs[i_b, i_se].min, pos_v)
                 aabbs[i_b, i_se].max = ti.max(aabbs[i_b, i_se].max, pos_v)
 
@@ -729,18 +729,16 @@ def compute_rigid_pcg_matrix_vector_product(self):
         )
 
     @ti.func
-    def compute_elastic_products(self, i_b, i_e, B, s, i_v0, i_v1, i_v2, i_v3, src):
+    def compute_elastic_products(self, i_b, i_e, S, i_vs, src):
         p9 = ti.Vector.zero(gs.ti_float, 9)
-        for i in ti.static(range(3)):
-            p9[i * 3 : i * 3 + 3] = (
-                B[0, i] * src[i_b, i_v0] + B[1, i] * src[i_b, i_v1] + B[2, i] * src[i_b, i_v2] + s[i] * src[i_b, i_v3]
-            )
+        for i, j in ti.static(ti.ndrange(3, 4)):
+            p9[i * 3 : i * 3 + 3] = p9[i * 3 : i * 3 + 3] + (S[j, i] * src[i_b, i_vs[j]])
+
         H9_p9 = ti.Vector.zero(gs.ti_float, 9)
-        for i in ti.static(range(3)):
-            H9_p9[i * 3 : i * 3 + 3] = (
-                self.fem_solver.elements_el_hessian[i_b, i, 0, i_e] @ p9[0:3]
-                + self.fem_solver.elements_el_hessian[i_b, i, 1, i_e] @ p9[3:6]
-                + self.fem_solver.elements_el_hessian[i_b, i, 2, i_e] @ p9[6:9]
+
+        for i, j in ti.static(ti.ndrange(3, 3)):
+            H9_p9[i * 3 : i * 3 + 3] = H9_p9[i * 3 : i * 3 + 3] + (
+                self.fem_solver.elements_el_hessian[i_b, i, j, i_e] @ p9[j * 3 : j * 3 + 3]
             )
         return p9, H9_p9
 
@@ -767,16 +765,21 @@ def compute_fem_matrix_vector_product(self, src, dst, active):
                 continue
             V_dt2 = self.fem_solver.elements_i[i_e].V * dt2
             B = self.fem_solver.elements_i[i_e].B
-            s = -B[0, :] - B[1, :] - B[2, :]  # s is the negative sum of B rows
-            i_v0, i_v1, i_v2, i_v3 = self.fem_solver.elements_i[i_e].el2v
+            S = ti.Matrix.zero(gs.ti_float, 4, 3)
+            S[:3, :] = B
+            S[3, :] = -B[0, :] - B[1, :] - B[2, :]
+            i_vs = self.fem_solver.elements_i[i_e].el2v
+
+            if ti.static(self.fem_solver._enable_vertex_constraints):
+                for i in ti.static(range(4)):
+                    if self.fem_solver.vertex_constraints.is_constrained[i_vs[i], i_b]:
+                        S[i, :] = ti.Vector.zero(gs.ti_float, 3)
 
-            _, new_p9 = self.compute_elastic_products(i_b, i_e, B, s, i_v0, i_v1, i_v2, i_v3, src)
+            _, new_p9 = self.compute_elastic_products(i_b, i_e, S, i_vs, src)
             # atomic
             scale = V_dt2 * damping_beta_factor
-            dst[i_b, i_v0] += (B[0, 0] * new_p9[0:3] + B[0, 1] * new_p9[3:6] + B[0, 2] * new_p9[6:9]) * scale
-            dst[i_b, i_v1] += (B[1, 0] * new_p9[0:3] + B[1, 1] * new_p9[3:6] + B[1, 2] * new_p9[6:9]) * scale
-            dst[i_b, i_v2] += (B[2, 0] * new_p9[0:3] + B[2, 1] * new_p9[3:6] + B[2, 2] * new_p9[6:9]) * scale
-            dst[i_b, i_v3] += (s[0] * new_p9[0:3] + s[1] * new_p9[3:6] + s[2] * new_p9[6:9]) * scale
+            for i in ti.static(range(4)):
+                dst[i_b, i_vs[i]] += (S[i, 0] * new_p9[0:3] + S[i, 1] * new_p9[3:6] + S[i, 2] * new_p9[6:9]) * scale
 
     def init_pcg_solve(self):
         self.init_pcg_state()
@@ -1101,10 +1104,17 @@ def compute_fem_energy(self, i_step: ti.i32, energy: ti.template()):
 
             V_dt2 = self.fem_solver.elements_i[i_e].V * dt2
             B = self.fem_solver.elements_i[i_e].B
-            s = -B[0, :] - B[1, :] - B[2, :]  # s is the negative sum of B rows
-            i_v0, i_v1, i_v2, i_v3 = self.fem_solver.elements_i[i_e].el2v
+            S = ti.Matrix.zero(gs.ti_float, 4, 3)
+            S[:3, :] = B
+            S[3, :] = -B[0, :] - B[1, :] - B[2, :]
+            i_vs = self.fem_solver.elements_i[i_e].el2v
+
+            if ti.static(self.fem_solver._enable_vertex_constraints):
+                for i in ti.static(range(4)):
+                    if self.fem_solver.vertex_constraints.is_constrained[i_vs[i], i_b]:
+                        S[i, :] = ti.Vector.zero(gs.ti_float, 3)
 
-            p9, H9_p9 = self.compute_elastic_products(i_b, i_e, B, s, i_v0, i_v1, i_v2, i_v3, self.fem_state_v.v_diff)
+            p9, H9_p9 = self.compute_elastic_products(i_b, i_e, S, i_vs, self.fem_state_v.v_diff)
             energy[i_b] += 0.5 * p9.dot(H9_p9) * damping_beta_factor * V_dt2
 
     @ti.func
@@ -1991,7 +2001,11 @@ def add_Jt_x(self, y, i_p, x):
         i_g = self.contact_pairs[i_p].geom_idx
         for i in ti.static(range(4)):
             i_v = self.fem_solver.elements_i[i_g].el2v[i]
-            y[i_b, i_v] += self.contact_pairs[i_p].barycentric[i] * x
+            if ti.static(self.fem_solver._enable_vertex_constraints):
+                if not self.fem_solver.vertex_constraints.is_constrained[i_v, i_b]:
+                    y[i_b, i_v] += self.contact_pairs[i_p].barycentric[i] * x
+            else:
+                y[i_b, i_v] += self.contact_pairs[i_p].barycentric[i] * x
 
     @ti.func
     def add_Jt_A_J_diag3x3(self, y, i_p, A):
@@ -1999,7 +2013,11 @@ def add_Jt_A_J_diag3x3(self, y, i_p, A):
         i_g = self.contact_pairs[i_p].geom_idx
         for i in ti.static(range(4)):
             i_v = self.fem_solver.elements_i[i_g].el2v[i]
-            y[i_b, i_v] += self.contact_pairs[i_p].barycentric[i] ** 2 * A
+            if ti.static(self.fem_solver._enable_vertex_constraints):
+                if not self.fem_solver.vertex_constraints.is_constrained[i_v, i_b]:
+                    y[i_b, i_v] += self.contact_pairs[i_p].barycentric[i] ** 2 * A
+            else:
+                y[i_b, i_v] += self.contact_pairs[i_p].barycentric[i] ** 2 * A
 
     @ti.func
     def compute_delassus(self, i_p):
@@ -2302,10 +2320,18 @@ def add_Jt_x(self, y, i_p, x):
         x_ = world @ x
         for i in ti.static(range(4)):
             i_v = self.fem_solver.elements_i[i_g0].el2v[i]
-            y[i_b, i_v] += self.contact_pairs[i_p].barycentric0[i] * x_
+            if ti.static(self.fem_solver._enable_vertex_constraints):
+                if not self.fem_solver.vertex_constraints.is_constrained[i_v, i_b]:
+                    y[i_b, i_v] += self.contact_pairs[i_p].barycentric0[i] * x_
+            else:
+                y[i_b, i_v] += self.contact_pairs[i_p].barycentric0[i] * x_
         for i in ti.static(range(4)):
             i_v = self.fem_solver.elements_i[i_g1].el2v[i]
-            y[i_b, i_v] -= self.contact_pairs[i_p].barycentric1[i] * x_
+            if ti.static(self.fem_solver._enable_vertex_constraints):
+                if not self.fem_solver.vertex_constraints.is_constrained[i_v, i_b]:
+                    y[i_b, i_v] -= self.contact_pairs[i_p].barycentric1[i] * x_
+            else:
+                y[i_b, i_v] -= self.contact_pairs[i_p].barycentric1[i] * x_
 
     @ti.func
     def add_Jt_A_J_diag3x3(self, y, i_p, A):
@@ -2318,10 +2344,18 @@ def add_Jt_A_J_diag3x3(self, y, i_p, A):
         B_ = world @ A @ world.transpose()
         for i in ti.static(range(4)):
             i_v = self.fem_solver.elements_i[i_g0].el2v[i]
-            y[i_b, i_v] += self.contact_pairs[i_p].barycentric0[i] ** 2 * B_
+            if ti.static(self.fem_solver._enable_vertex_constraints):
+                if not self.fem_solver.vertex_constraints.is_constrained[i_v, i_b]:
+                    y[i_b, i_v] += self.contact_pairs[i_p].barycentric0[i] ** 2 * B_
+            else:
+                y[i_b, i_v] += self.contact_pairs[i_p].barycentric0[i] ** 2 * B_
         for i in ti.static(range(4)):
             i_v = self.fem_solver.elements_i[i_g1].el2v[i]
-            y[i_b, i_v] += self.contact_pairs[i_p].barycentric1[i] ** 2 * B_
+            if ti.static(self.fem_solver._enable_vertex_constraints):
+                if not self.fem_solver.vertex_constraints.is_constrained[i_v, i_b]:
+                    y[i_b, i_v] += self.contact_pairs[i_p].barycentric1[i] ** 2 * B_
+            else:
+                y[i_b, i_v] += self.contact_pairs[i_p].barycentric1[i] ** 2 * B_
 
     @ti.func
     def compute_delassus(self, i_p):
@@ -2408,13 +2442,21 @@ def compute_Jx(self, i_p, x):
     def add_Jt_x(self, y, i_p, x):
         i_b = self.contact_pairs[i_p].batch_idx
         i_g = self.contact_pairs[i_p].geom_idx
-        y[i_b, i_g] += x
+        if ti.static(self.fem_solver._enable_vertex_constraints):
+            if not self.fem_solver.vertex_constraints.is_constrained[i_g, i_b]:
+                y[i_b, i_g] += x
+        else:
+            y[i_b, i_g] += x
 
     @ti.func
     def add_Jt_A_J_diag3x3(self, y, i_p, A):
         i_b = self.contact_pairs[i_p].batch_idx
         i_g = self.contact_pairs[i_p].geom_idx
-        y[i_b, i_g] += A
+        if ti.static(self.fem_solver._enable_vertex_constraints):
+            if not self.fem_solver.vertex_constraints.is_constrained[i_g, i_b]:
+                y[i_b, i_g] += A
+        else:
+            y[i_b, i_g] += A
 
     @ti.func
     def compute_delassus(self, i_p):
diff --git a/genesis/engine/entities/fem_entity.py b/genesis/engine/entities/fem_entity.py
index cf41df3abd..35c1ac3fe2 100644
--- a/genesis/engine/entities/fem_entity.py
+++ b/genesis/engine/entities/fem_entity.py
@@ -803,8 +803,9 @@ def set_vertex_constraints(
                 List of environment indices to apply the constraints to. If None, applies to all environments.
         """
         if self._solver._use_implicit_solver:
-            gs.logger.warning("Ignoring vertex constraint; unsupported with FEM implicit solver.")
-            return
+            if not self._solver._enable_vertex_constraints:
+                gs.logger.warning("Ignoring vertex constraint; FEM implicit solver needs to enable vertex constraints.")
+                return
 
         if not self._solver._constraints_initialized:
             self._solver.init_constraints()
diff --git a/genesis/engine/solvers/fem_solver.py b/genesis/engine/solvers/fem_solver.py
index f5acb5307e..d088dd39bf 100644
--- a/genesis/engine/solvers/fem_solver.py
+++ b/genesis/engine/solvers/fem_solver.py
@@ -37,6 +37,7 @@ def __init__(self, scene, sim, options):
         self._linesearch_tau = options.linesearch_tau
         self._damping_alpha = options.damping_alpha
         self._damping_beta = options.damping_beta
+        self._enable_vertex_constraints = options.enable_vertex_constraints
 
         # use scaled volume for better numerical stability, similar to p_vol_scale in mpm
         self._vol_scale = float(1e4)
@@ -399,6 +400,9 @@ def build(self):
                     "Please check the input mesh or the FEM solver implementation."
                 )
 
+        if self._enable_vertex_constraints and not self._constraints_initialized:
+            self.init_constraints()
+
     def add_entity(self, idx, material, morph, surface):
         # add material's update methods if not matching any existing material
         exist = False
@@ -514,12 +518,24 @@ def precompute_material_data(self, f: ti.i32):
     def init_pos_and_inertia(self, f: ti.i32):
         dt2 = self.substep_dt**2
         for i_v, i_b in ti.ndrange(self.n_vertices, self._B):
-            self.elements_v_energy[i_b, i_v].inertia = (
-                self.elements_v[f, i_v, i_b].pos
-                + self.elements_v[f, i_v, i_b].vel * self.substep_dt
-                + self._gravity[i_b] * dt2
-            )
-            self.elements_v[f + 1, i_v, i_b].pos = self.elements_v[f, i_v, i_b].pos
+            if ti.static(self._enable_vertex_constraints):
+                if self.vertex_constraints.is_constrained[i_v, i_b]:
+                    self.elements_v[f + 1, i_v, i_b].pos = self.vertex_constraints.target_pos[i_v, i_b]
+                    self.elements_v_energy[i_b, i_v].inertia = self.vertex_constraints.target_pos[i_v, i_b]
+                else:
+                    self.elements_v_energy[i_b, i_v].inertia = (
+                        self.elements_v[f, i_v, i_b].pos
+                        + self.elements_v[f, i_v, i_b].vel * self.substep_dt
+                        + self._gravity[i_b] * dt2
+                    )
+                    self.elements_v[f + 1, i_v, i_b].pos = self.elements_v[f, i_v, i_b].pos
+            else:
+                self.elements_v_energy[i_b, i_v].inertia = (
+                    self.elements_v[f, i_v, i_b].pos
+                    + self.elements_v[f, i_v, i_b].vel * self.substep_dt
+                    + self._gravity[i_b] * dt2
+                )
+                self.elements_v[f + 1, i_v, i_b].pos = self.elements_v[f, i_v, i_b].pos
 
     @ti.func
     def _compute_ele_J_F(self, f: ti.i32, i_e: ti.i32, i_b: ti.i32):
@@ -579,6 +595,21 @@ def compute_ele_hessian_gradient(self, f: ti.i32):
                             hessian_field=self.elements_el_hessian,
                         )
 
+    @ti.func
+    def _func_compute_element_mapping_matrix(self, i_vs, B, i_b):
+        """
+        Compute the element mapping matrix S for an element.
+        """
+        S = ti.Matrix.zero(gs.ti_float, 4, 3)
+        S[:3, :] = B
+        S[3, :] = -B[0, :] - B[1, :] - B[2, :]
+
+        if ti.static(self._enable_vertex_constraints):
+            for i in ti.static(range(4)):
+                if self.vertex_constraints.is_constrained[i_vs[i], i_b]:
+                    S[i, :] = ti.Vector.zero(gs.ti_float, 3)
+        return S
+
     @ti.func
     def _func_compute_ele_energy(self, f: ti.i32):
         """
@@ -606,42 +637,14 @@ def _func_compute_ele_energy(self, f: ti.i32):
             # add linearized damping energy
             if self._damping_beta > gs.EPS:
                 damping_beta_over_dt = self._damping_beta / self._substep_dt
-                i_v = self.elements_i[i_e].el2v
-                S = ti.Matrix.zero(gs.ti_float, 4, 3)
+                i_vs = self.elements_i[i_e].el2v
                 B = self.elements_i[i_e].B
-                S[:3, :] = B
-                S[3, :] = -B[0, :] - B[1, :] - B[2, :]
+                S = self._func_compute_element_mapping_matrix(i_vs, B, i_b)
 
                 x_diff = ti.Vector.zero(gs.ti_float, 12)
                 for i in ti.static(range(4)):
                     x_diff[i * 3 : i * 3 + 3] = (
-                        self.elements_v[f + 1, i_v[i], i_b].pos - self.elements_v[f, i_v[i], i_b].pos
-                    )
-                St_x_diff = ti.Vector.zero(gs.ti_float, 9)
-                for i, j in ti.static(ti.ndrange(3, 4)):
-                    St_x_diff[i * 3 : i * 3 + 3] += S[j, i] * x_diff[j * 3 : j * 3 + 3]
-
-                H_St_x_diff = ti.Vector.zero(gs.ti_float, 9)
-                for i, j in ti.static(ti.ndrange(3, 3)):
-                    H_St_x_diff[i * 3 : i * 3 + 3] += (
-                        self.elements_el_hessian[i_b, i, j, i_e] @ St_x_diff[j * 3 : j * 3 + 3]
-                    )
-
-                self.elements_el_energy[i_b, i_e].energy += 0.5 * damping_beta_over_dt * St_x_diff.dot(H_St_x_diff)
-
-            # add linearized damping energy
-            if self._damping_beta > gs.EPS:
-                damping_beta_over_dt = self._damping_beta / self._substep_dt
-                i_v = self.elements_i[i_e].el2v
-                S = ti.Matrix.zero(gs.ti_float, 4, 3)
-                B = self.elements_i[i_e].B
-                S[:3, :] = B
-                S[3, :] = -B[0, :] - B[1, :] - B[2, :]
-
-                x_diff = ti.Vector.zero(gs.ti_float, 12)
-                for i in ti.static(range(4)):
-                    x_diff[i * 3 : i * 3 + 3] = (
-                        self.elements_v[f + 1, i_v[i], i_b].pos - self.elements_v[f, i_v[i], i_b].pos
+                        self.elements_v[f + 1, i_vs[i], i_b].pos - self.elements_v[f, i_vs[i], i_b].pos
                     )
                 St_x_diff = ti.Vector.zero(gs.ti_float, 9)
                 for i, j in ti.static(ti.ndrange(3, 4)):
@@ -682,23 +685,19 @@ def accumulate_vertex_force_preconditioner(self, f: ti.i32):
             V = self.elements_i[i_e].V
             B = self.elements_i[i_e].B
             gradient = self.elements_el_energy[i_b, i_e].gradient
-            force = -V * gradient @ B.transpose()
-            i_v = self.elements_i[i_e].el2v
+            i_vs = self.elements_i[i_e].el2v
+            S = self._func_compute_element_mapping_matrix(i_vs, B, i_b)
+            force = -V * gradient @ S.transpose()
 
             # atomic
-            self.elements_v_energy[i_b, i_v[0]].force += force[:, 0]
-            self.elements_v_energy[i_b, i_v[1]].force += force[:, 1]
-            self.elements_v_energy[i_b, i_v[2]].force += force[:, 2]
-            self.elements_v_energy[i_b, i_v[3]].force -= force[:, 0] + force[:, 1] + force[:, 2]
-            S = ti.Matrix.zero(gs.ti_float, 4, 3)
-            S[:3, :] = B
-            S[3, :] = -B[0, :] - B[1, :] - B[2, :]
+            for i in ti.static(range(4)):
+                self.elements_v_energy[i_b, i_vs[i]].force += force[:, i]
 
             if self._damping_beta > gs.EPS:
                 x_diff = ti.Vector.zero(gs.ti_float, 12)
                 for i in ti.static(range(4)):
                     x_diff[i * 3 : i * 3 + 3] = (
-                        self.elements_v[f + 1, i_v[i], i_b].pos - self.elements_v[f, i_v[i], i_b].pos
+                        self.elements_v[f + 1, i_vs[i], i_b].pos - self.elements_v[f, i_vs[i], i_b].pos
                     )
                 St_x_diff = ti.Vector.zero(gs.ti_float, 9)
                 for i, j in ti.static(ti.ndrange(3, 4)):
@@ -713,13 +712,13 @@ def accumulate_vertex_force_preconditioner(self, f: ti.i32):
                 for i, j in ti.static(ti.ndrange(4, 3)):
                     S_H_St_x_diff[i * 3 : i * 3 + 3] += S[i, j] * H_St_x_diff[j * 3 : j * 3 + 3]
                 for i in ti.static(range(4)):
-                    self.elements_v_energy[i_b, i_v[i]].force += (
+                    self.elements_v_energy[i_b, i_vs[i]].force += (
                         -damping_beta_over_dt * V * S_H_St_x_diff[i * 3 : i * 3 + 3]
                     )
 
             # diagonal 3-by-3 block of hessian
             for k, i, j in ti.ndrange(4, 3, 3):
-                self.pcg_state_v[i_b, i_v[k]].diag3x3 += (
+                self.pcg_state_v[i_b, i_vs[k]].diag3x3 += (
                     V * damping_beta_factor * S[k, i] * S[k, j] * self.elements_el_hessian[i_b, i, j, i_e]
                 )
 
@@ -758,40 +757,26 @@ def compute_Ap(self):
                 continue
             V = self.elements_i[i_e].V
             B = self.elements_i[i_e].B
-            s = -B[0, :] - B[1, :] - B[2, :]  # s is the negative sum of B rows
+            i_vs = self.elements_i[i_e].el2v
+            S = self._func_compute_element_mapping_matrix(i_vs, B, i_b)
+
             p9 = ti.Vector([0.0] * 9, dt=gs.ti_float)
-            i_v0, i_v1, i_v2, i_v3 = self.elements_i[i_e].el2v
 
-            for i in ti.static(range(3)):
-                p9[i * 3 : i * 3 + 3] = (
-                    B[0, i] * self.pcg_state_v[i_b, i_v0].p
-                    + B[1, i] * self.pcg_state_v[i_b, i_v1].p
-                    + B[2, i] * self.pcg_state_v[i_b, i_v2].p
-                    + s[i] * self.pcg_state_v[i_b, i_v3].p
-                )
+            for i, j in ti.static(ti.ndrange(3, 4)):
+                p9[i * 3 : i * 3 + 3] = p9[i * 3 : i * 3 + 3] + S[j, i] * self.pcg_state_v[i_b, i_vs[j]].p
 
             new_p9 = ti.Vector([0.0] * 9, dt=gs.ti_float)
 
-            for i in ti.static(range(3)):
+            for i, j in ti.static(ti.ndrange(3, 3)):
                 new_p9[i * 3 : i * 3 + 3] = (
-                    self.elements_el_hessian[i_b, i, 0, i_e] @ p9[0:3]
-                    + self.elements_el_hessian[i_b, i, 1, i_e] @ p9[3:6]
-                    + self.elements_el_hessian[i_b, i, 2, i_e] @ p9[6:9]
+                    new_p9[i * 3 : i * 3 + 3] + self.elements_el_hessian[i_b, i, j, i_e] @ p9[j * 3 : j * 3 + 3]
                 )
 
             # atomic
-            self.pcg_state_v[i_b, i_v0].Ap += (
-                (B[0, 0] * new_p9[0:3] + B[0, 1] * new_p9[3:6] + B[0, 2] * new_p9[6:9]) * V * damping_beta_factor
-            )
-            self.pcg_state_v[i_b, i_v1].Ap += (
-                (B[1, 0] * new_p9[0:3] + B[1, 1] * new_p9[3:6] + B[1, 2] * new_p9[6:9]) * V * damping_beta_factor
-            )
-            self.pcg_state_v[i_b, i_v2].Ap += (
-                (B[2, 0] * new_p9[0:3] + B[2, 1] * new_p9[3:6] + B[2, 2] * new_p9[6:9]) * V * damping_beta_factor
-            )
-            self.pcg_state_v[i_b, i_v3].Ap += (
-                (s[0] * new_p9[0:3] + s[1] * new_p9[3:6] + s[2] * new_p9[6:9]) * V * damping_beta_factor
-            )
+            for i in ti.static(range(4)):
+                self.pcg_state_v[i_b, i_vs[i]].Ap += (
+                    (S[i, 0] * new_p9[0:3] + S[i, 1] * new_p9[3:6] + S[i, 2] * new_p9[6:9]) * V * damping_beta_factor
+                )
 
     @ti.kernel
     def init_pcg_solve(self):
diff --git a/genesis/options/solvers.py b/genesis/options/solvers.py
index b9d7e81f5f..7908624197 100644
--- a/genesis/options/solvers.py
+++ b/genesis/options/solvers.py
@@ -114,6 +114,11 @@ class SAPCouplerOptions(BaseCouplerOptions):
     """
     Options configuring the inter-solver coupling for the Semi-Analytic Primal (SAP) contact solver used in Drake.
 
+    Note
+    ----
+    Paper reference: https://arxiv.org/abs/2110.10107
+    Drake reference: https://drake.mit.edu/release_notes/v1.5.0.html
+
     Parameters
     ----------
     n_sap_iterations : int, optional
@@ -152,10 +157,6 @@ class SAPCouplerOptions(BaseCouplerOptions):
         Type of contact against the floor for rigid bodies. Defaults to "vert". Can be "vert" or "none".
     enable_rigid_fem_contact : bool, optional
         Whether to enable coupling between rigid and FEM solvers. Defaults to True.
-    Note
-    ----
-    Paper reference: https://arxiv.org/abs/2110.10107
-    Drake reference: https://drake.mit.edu/release_notes/v1.5.0.html
     """
 
     n_sap_iterations: int = 5
@@ -581,6 +582,13 @@ class FEMOptions(Options):
     """
     Options configuring the FEMSolver.
 
+    Note
+    ----
+    - Damping coefficients are used to control the damping effect in the simulation.
+    They are used in the Rayleigh Damping model, which is a common damping model in FEM simulations.
+    Reference: https://doc.comsol.com/5.5/doc/com.comsol.help.sme/sme_ug_modeling.05.083.html
+    - TODO Move it to material parameters in the future instead of solver options.
+
     Parameters
     ----------
     dt : float, optional
@@ -612,13 +620,8 @@ class FEMOptions(Options):
         Rayleigh Damping factor for the implicit solver. Defaults to 0.5. Only used when `use_implicit_solver` is True.
     damping_beta : float, optional
         Rayleigh Damping factor for the implicit solver. Defaults to 5e-4. Only used when `use_implicit_solver` is True.
-
-    Note
-    ----
-    - Damping coefficients are used to control the damping effect in the simulation.
-    They are used in the Rayleigh Damping model, which is a common damping model in FEM simulations.
-    Reference: https://doc.comsol.com/5.5/doc/com.comsol.help.sme/sme_ug_modeling.05.083.html
-    - TODO Move it to material parameters in the future instead of solver options.
+    enable_vertex_constraints : bool, optional
+        Whether to enable vertex constraints. Defaults to False.
     """
 
     dt: Optional[float] = None
@@ -635,6 +638,7 @@ class FEMOptions(Options):
     linesearch_tau: float = 0.5
     damping_alpha: float = 0.5
     damping_beta: float = 5e-4
+    enable_vertex_constraints: bool = False
 
 
 class SFOptions(Options):
diff --git a/tests/test_fem.py b/tests/test_fem.py
index 8f515183d3..29e127d7fb 100644
--- a/tests/test_fem.py
+++ b/tests/test_fem.py
@@ -582,3 +582,171 @@ def test_fem_articulated(fem_material_linear_corotated_soft, show_viewer):
         atol=0.2,
         err_msg=f"Link center {center} moves too far from [-0.5, -0.5, 0.04].",
     )
+
+
+def test_implicit_hard_vertex_constraint(fem_material_linear_corotated, show_viewer):
+    """
+    Test if a box with hard vertex constraints has those vertices fixed, and that updating and removing constraints
+    works correctly.
+    """
+    scene = gs.Scene(
+        sim_options=gs.options.SimOptions(
+            dt=1 / 60,
+            substeps=1,
+        ),
+        fem_options=gs.options.FEMOptions(
+            use_implicit_solver=True,
+            enable_vertex_constraints=True,
+        ),
+        coupler_options=gs.options.SAPCouplerOptions(),
+        show_viewer=show_viewer,
+        show_FPS=False,
+    )
+
+    asset_path = get_hf_dataset(pattern="meshes/cube8.obj")
+    cube = scene.add_entity(
+        morph=gs.morphs.Mesh(
+            file=f"{asset_path}/meshes/cube8.obj",
+            scale=0.1,
+            pos=np.array([0.0, 0.0, 0.6], dtype=np.float32),
+        ),
+        material=fem_material_linear_corotated,
+    )
+
+    verts_idx = [0]
+    initial_target_poss = cube.init_positions[verts_idx]
+
+    scene.build()
+
+    if show_viewer:
+        sphere = scene.draw_debug_spheres(poss=initial_target_poss, radius=0.02, color=(1, 0, 1, 0.8))
+
+    cube.set_vertex_constraints(verts_idx=verts_idx, target_poss=initial_target_poss)
+
+    for _ in range(100):
+        scene.step()
+
+    positions = cube.get_state().pos[0][verts_idx]
+    assert_allclose(
+        positions, initial_target_poss, tol=gs.EPS
+    ), "Vertices should stay at initial target positions with hard constraints"
+    new_target_poss = initial_target_poss + gs.tensor(
+        [[0.1, 0.1, 0.1]],
+    )
+    cube.update_constraint_targets(verts_idx=verts_idx, target_poss=new_target_poss)
+    if show_viewer:
+        scene.clear_debug_object(sphere)
+        sphere = scene.draw_debug_spheres(poss=new_target_poss, radius=0.02, color=(1, 0, 1, 0.8))
+    for _ in range(100):
+        scene.step()
+
+    positions_after_update = cube.get_state().pos[0][verts_idx]
+    assert_allclose(
+        positions_after_update, new_target_poss, tol=gs.EPS
+    ), "Vertices should be at new target positions after updating constraints"
+
+    cube.remove_vertex_constraints()
+    if show_viewer:
+        scene.clear_debug_object(sphere)
+
+    for _ in range(100):
+        scene.step()
+
+    state = cube.get_state()
+    center = state.pos.mean(axis=(0, 1))
+    assert_allclose(
+        center,
+        np.array([0.2, 0.13, 0.1], dtype=np.float32),
+        atol=0.2,
+        err_msg=f"Cube center {center} moves too far from [0.2, 0.13, 0.1] after removing constraints.",
+    )
+
+    velocity = state.vel.mean(axis=(0, 1))
+    assert_allclose(
+        velocity,
+        np.array([0.0, 0.0, 0.0], dtype=np.float32),
+        atol=1e-5,
+        err_msg=f"Cube velocity {velocity} should be close to zero after settling.",
+    )
+
+    # The contact requires some penetration to generate enough contact force to cancel out gravity
+    min_pos_z = state.pos[..., 2].min()
+    assert_allclose(min_pos_z, -2.0e-5, atol=5e-6), f"Cube minimum Z position {min_pos_z} is not close to -2.0e-5."
+
+
+def test_sphere_box_vertex_constraint(fem_material_linear_corotated, show_viewer):
+    """
+    Test if a box with hard vertex constraints has those vertices fixed, and collisiong with a sphere works correctly.
+    """
+    scene = gs.Scene(
+        sim_options=gs.options.SimOptions(
+            dt=1 / 60,
+            substeps=1,
+        ),
+        fem_options=gs.options.FEMOptions(
+            use_implicit_solver=True,
+            enable_vertex_constraints=True,
+        ),
+        coupler_options=gs.options.SAPCouplerOptions(),
+        show_viewer=show_viewer,
+        show_FPS=False,
+    )
+
+    asset_path = get_hf_dataset(pattern="meshes/cube8.obj")
+    cube = scene.add_entity(
+        morph=gs.morphs.Mesh(
+            file=f"{asset_path}/meshes/cube8.obj",
+            scale=0.1,
+            pos=np.array([0.0, 0.0, 0.35], dtype=np.float32),
+        ),
+        material=fem_material_linear_corotated,
+    )
+
+    verts_idx = [0]
+    initial_target_poss = cube.init_positions[verts_idx]
+
+    sphere = scene.add_entity(
+        morph=gs.morphs.Sphere(
+            pos=(0.0, 0.0, 0.1),
+            radius=0.1,
+        ),
+        material=fem_material_linear_corotated,
+    )
+
+    scene.build()
+    if show_viewer:
+        sphere_debug = scene.draw_debug_spheres(poss=initial_target_poss, radius=0.02, color=(1, 0, 1, 0.8))
+
+    cube.set_vertex_constraints(verts_idx=verts_idx, target_poss=initial_target_poss)
+
+    for _ in range(200):
+        scene.step()
+
+    pos = cube.get_state().pos
+    fixed_pos = pos[0][verts_idx]
+    assert_allclose(
+        fixed_pos, initial_target_poss, tol=gs.EPS
+    ), "Vertices should stay at initial target positions with hard constraints"
+
+    state = sphere.get_state()
+    center = state.pos.mean(axis=(0, 1))
+    assert_allclose(
+        center,
+        np.array([0.4, 0.4, 0.1], dtype=np.float32),
+        atol=0.2,
+        err_msg=f"Sphere center {center} moved too much from initial position [0.4, 0.4, 0.1].",
+    )
+
+    # Using a larger tolerance here since the sphere is rolling, rolling friction is not accurately modeled.
+    velocity = state.vel.mean(axis=(0, 1))
+    assert_allclose(
+        velocity,
+        np.array([0.0, 0.0, 0.0], dtype=np.float32),
+        atol=0.03,
+        err_msg=f"Sphere velocity {velocity} should be close to zero after settling.",
+    )
+
+    min_sphere_pos_z = state.pos[..., 2].min()
+    assert_allclose(
+        min_sphere_pos_z, -1e-3, atol=2e-4
+    ), f"Sphere minimum Z position {min_sphere_pos_z} is not close to cube bottom surface."