diff --git a/README.md b/README.md index b19785302b..0b62cb2baf 100644 --- a/README.md +++ b/README.md @@ -194,6 +194,39 @@ uv sync --extra examples +### Cable Examples + + + + + + + + + + + + +
+ + Cable Bend + + + + Cable Twist + + + + Cable Bundle Hysteresis + +
+ uv run -m newton.examples cable_bend + + uv run -m newton.examples cable_twist + + uv run -m newton.examples cable_bundle_hysteresis +
+ ### Cloth Examples diff --git a/docs/images/examples/example_cable_bend.jpg b/docs/images/examples/example_cable_bend.jpg new file mode 100644 index 0000000000..312b3987cb Binary files /dev/null and b/docs/images/examples/example_cable_bend.jpg differ diff --git a/docs/images/examples/example_cable_bundle_hysteresis.jpg b/docs/images/examples/example_cable_bundle_hysteresis.jpg new file mode 100644 index 0000000000..1d59d24c89 Binary files /dev/null and b/docs/images/examples/example_cable_bundle_hysteresis.jpg differ diff --git a/docs/images/examples/example_cable_twist.jpg b/docs/images/examples/example_cable_twist.jpg new file mode 100644 index 0000000000..8cfa35310b Binary files /dev/null and b/docs/images/examples/example_cable_twist.jpg differ diff --git a/newton/_src/sim/builder.py b/newton/_src/sim/builder.py index 57d783a884..bcce19a388 100644 --- a/newton/_src/sim/builder.py +++ b/newton/_src/sim/builder.py @@ -2951,10 +2951,15 @@ def add_joint_cable( translation is the attachment point. child_xform (Transform): The transform of the joint in the child body's local frame; its translation is the attachment point. - stretch_stiffness: Linear stretch stiffness. If None, defaults to 1.0e9. - stretch_damping: Linear stretch damping. If None, defaults to 0.0. - bend_stiffness: Angular bend/twist stiffness. If None, defaults to 0.0. - bend_damping: Angular bend/twist damping. If None, defaults to 0.0. + stretch_stiffness: Cable stretch stiffness (stored as ``target_ke``) [N/m]. If None, defaults to 1.0e9. + stretch_damping: Cable stretch damping (stored as ``target_kd``). In :class:`newton.solvers.SolverVBD` + this is a dimensionless (Rayleigh-style) coefficient. If None, + defaults to 0.0. + bend_stiffness: Cable bend/twist stiffness (stored as ``target_ke``) [N*m] (torque per radian). If None, + defaults to 0.0. + bend_damping: Cable bend/twist damping (stored as ``target_kd``). In :class:`newton.solvers.SolverVBD` + this is a dimensionless (Rayleigh-style) coefficient. If None, + defaults to 0.0. key: The key of the joint. collision_filter_parent: Whether to filter collisions between shapes of the parent and child bodies. enabled: Whether the joint is enabled. @@ -4664,10 +4669,18 @@ def add_rod( align the capsule's local +Z with the segment direction ``positions[i+1] - positions[i]``. radius: Capsule radius. cfg: Shape configuration for the capsules. If None, :attr:`default_shape_cfg` is used. - stretch_stiffness: Stretch stiffness for the cable joints. If None, defaults to 1.0e9. - stretch_damping: Stretch damping for the cable joints. If None, defaults to 0.0. - bend_stiffness: Bend/twist stiffness for the cable joints. If None, defaults to 0.0. - bend_damping: Bend/twist damping for the cable joints. If None, defaults to 0.0. + stretch_stiffness: Stretch stiffness for the cable joints. For rods, this is treated as a + material-like axial/shear stiffness (commonly interpreted as EA) + with units [N] and is internally converted to an effective point stiffness [N/m] by dividing by + segment length. If None, defaults to 1.0e9. + stretch_damping: Stretch damping for the cable joints (applied per-joint; not length-normalized). If None, + defaults to 0.0. + bend_stiffness: Bend/twist stiffness for the cable joints. For rods, this is treated as a + material-like bending/twist stiffness (e.g., EI) with units [N*m^2] and is internally converted to + an effective per-joint stiffness [N*m] (torque per radian) by dividing by segment length. If None, + defaults to 0.0. + bend_damping: Bend/twist damping for the cable joints (applied per-joint; not length-normalized). If None, + defaults to 0.0. closed: If True, connects the last segment back to the first to form a closed loop. If False, creates an open chain. Note: rods require at least 2 segments. key: Optional key prefix for bodies, shapes, and joints. @@ -4692,6 +4705,9 @@ def add_rod( Note: - Bend defaults are 0.0 (no bending resistance unless specified). Stretch defaults to a high stiffness (1.0e9), which keeps neighboring capsules closely coupled (approximately inextensible). + - Internally, stretch and bend stiffnesses are pre-scaled by dividing by segment length so solver kernels + do not need per-segment length normalization. + - Damping values are passed through as provided (per joint) and are not length-normalized. - Each segment is implemented as a capsule primitive. The segment's body transform is placed at the start point ``positions[i]`` with a local center-of-mass offset of ``(0, 0, half_height)`` so that the COM lies at the segment midpoint. The capsule shape @@ -4709,6 +4725,11 @@ def add_rod( bend_damping = 0.0 if bend_damping is None else bend_damping # Input validation + if stretch_stiffness < 0.0 or bend_stiffness < 0.0: + raise ValueError("add_rod: stretch_stiffness and bend_stiffness must be >= 0") + + # Guard against near-zero lengths: segment length is used to normalize stiffness later (EA/L, EI/L). + min_segment_length = 1.0e-9 num_segments = len(quaternions) if len(positions) != num_segments + 1: raise ValueError( @@ -4735,10 +4756,10 @@ def add_rod( # Calculate segment properties segment_length = wp.length(p1 - p0) - if segment_length <= 0.0: + if segment_length <= min_segment_length: raise ValueError( - f"add_rod: segment {i} has zero or negative length; " - "positions must form strictly positive-length segments" + f"add_rod: segment {i} has a too-small length (length={float(segment_length):.3e}); " + f"segment length must be > {min_segment_length:.1e}" ) segment_lengths.append(float(segment_length)) half_height = 0.5 * segment_length @@ -4808,14 +4829,25 @@ def add_rod( # Joint key: numbered 1 through num_joints joint_key = f"{key}_cable_{i + 1}" if key else None + # Pre-scale rod stiffnesses here so solver kernels do not need per-segment length normalization. + # Use the parent segment length L. + # + # - Stretch: treat the user input as a material-like axial/shear stiffness (commonly EA) [N] + # and store an effective per-joint (point-to-point) spring stiffness k_eff = EA / L [N/m]. + # - Bend/twist: treat the user input as a material-like bending/twist stiffness (commonly EI) [N*m^2] + # and store an effective per-joint angular stiffness k_eff = EI / L [N*m]. + seg_len = segment_lengths[parent_idx] + stretch_ke_eff = stretch_stiffness / seg_len + bend_ke_eff = bend_stiffness / seg_len + joint = self.add_joint_cable( parent=parent_body, child=child_body, parent_xform=parent_xform, child_xform=child_xform, - bend_stiffness=bend_stiffness, + bend_stiffness=bend_ke_eff, bend_damping=bend_damping, - stretch_stiffness=stretch_stiffness, + stretch_stiffness=stretch_ke_eff, stretch_damping=stretch_damping, key=joint_key, collision_filter_parent=True, diff --git a/newton/_src/solvers/vbd/rigid_vbd_kernels.py b/newton/_src/solvers/vbd/rigid_vbd_kernels.py index 6c8b286871..bfa0e797f8 100644 --- a/newton/_src/solvers/vbd/rigid_vbd_kernels.py +++ b/newton/_src/solvers/vbd/rigid_vbd_kernels.py @@ -258,7 +258,7 @@ def compute_kappa_dot_analytic( @wp.func -def evaluate_cable_bend_force_hessian_avbd( +def evaluate_angular_constraint_force_hessian_isotropic( q_wp: wp.quat, q_wc: wp.quat, q_wp_rest: wp.quat, @@ -273,7 +273,7 @@ def evaluate_cable_bend_force_hessian_avbd( dt: float, ): """ - AVBD cable bend (component-wise Dahl): generalized torque and angular Hessian. + Isotropic rotational constraint force/Hessian using a 3D rotation-vector error (kappa) in the parent frame. Summary: - Curvature kappa from rest-aligned relative rotation @@ -367,7 +367,7 @@ def evaluate_cable_bend_force_hessian_avbd( @wp.func -def evaluate_cable_stretch_force_hessian_avbd( +def evaluate_linear_constraint_force_hessian_isotropic( X_wp: wp.transform, X_wc: wp.transform, X_wp_prev: wp.transform, @@ -382,7 +382,7 @@ def evaluate_cable_stretch_force_hessian_avbd( dt: float, ): """ - AVBD cable stretch: point-to-point pinning (C = x_c - x_p). + Isotropic linear point-to-point constraint: anchor coincidence (C = x_c - x_p). Summary: - Force at attachment point: f_attachment = k*C + (damping*k)*dC/dt @@ -856,21 +856,31 @@ def evaluate_joint_force_hessian( joint_child: wp.array(dtype=int), joint_X_p: wp.array(dtype=wp.transform), joint_X_c: wp.array(dtype=wp.transform), - joint_qd_start: wp.array(dtype=int), - joint_target_kd: wp.array(dtype=float), + joint_constraint_start: wp.array(dtype=int), joint_penalty_k: wp.array(dtype=float), + joint_penalty_kd: wp.array(dtype=float), joint_sigma_start: wp.array(dtype=wp.vec3), joint_C_fric: wp.array(dtype=wp.vec3), dt: float, ): - """Compute AVBD joint force and Hessian contributions for a specific body (cable joints). + """Compute AVBD joint force and Hessian contributions for one body (CABLE/BALL/FIXED). - Uses ``body_q_prev`` as the previous pose when estimating joint damping - over timestep ``dt``, while ``body_q`` and ``body_q_rest`` provide - current and rest configurations. Returns (force, torque, H_ll, H_al, H_aa) - in world frame. + Indexing: + ``joint_constraint_start[j]`` is a solver-owned start offset into the per-constraint arrays + (``joint_penalty_k``, ``joint_penalty_kd``). The layout is: + - ``JointType.CABLE``: 2 scalars -> [stretch (linear), bend (angular)] + - ``JointType.BALL``: 1 scalar -> [linear] + - ``JointType.FIXED``: 2 scalars -> [linear, angular] + + Damping: + ``joint_penalty_kd`` stores a dimensionless Rayleigh-style coefficient used to build a + physical damping term proportional to stiffness (e.g. D = kd * K). + + Uses ``body_q_prev`` to estimate constraint rates over timestep ``dt``; outputs are + (force, torque, H_ll, H_al, H_aa) in world frame. """ - if joint_type[joint_index] != JointType.CABLE: + jt = joint_type[joint_index] + if jt != JointType.CABLE and jt != JointType.BALL and jt != JointType.FIXED: return wp.vec3(0.0), wp.vec3(0.0), wp.mat33(0.0), wp.mat33(0.0), wp.mat33(0.0) parent_index = joint_parent[joint_index] @@ -899,52 +909,97 @@ def evaluate_joint_force_hessian( X_wp_rest = parent_pose_rest * X_pj X_wc_rest = child_pose_rest * X_cj - dof_start = joint_qd_start[joint_index] - stretch_penalty_k = joint_penalty_k[dof_start] - bend_penalty_k = joint_penalty_k[dof_start + 1] - stretch_damping = joint_target_kd[dof_start] - bend_damping = joint_target_kd[dof_start + 1] - - # Cable segment convention: parent_com[2] stores half-length in local frame - rest_length = 2.0 * parent_com[2] - if rest_length < _SMALL_LENGTH_EPS: - return wp.vec3(0.0), wp.vec3(0.0), wp.mat33(0.0), wp.mat33(0.0), wp.mat33(0.0) - - total_force = wp.vec3(0.0) - total_torque = wp.vec3(0.0) - total_H_ll = wp.mat33(0.0) - total_H_al = wp.mat33(0.0) - total_H_aa = wp.mat33(0.0) - - if bend_penalty_k > 0.0: - q_wp = wp.transform_get_rotation(X_wp) - q_wc = wp.transform_get_rotation(X_wc) - q_wp_rest = wp.transform_get_rotation(X_wp_rest) - q_wc_rest = wp.transform_get_rotation(X_wc_rest) - q_wp_prev = wp.transform_get_rotation(X_wp_prev) - q_wc_prev = wp.transform_get_rotation(X_wc_prev) - bend_k_eff = bend_penalty_k / rest_length - sigma0 = joint_sigma_start[joint_index] - C_fric = joint_C_fric[joint_index] - bend_torque, bend_H_aa = evaluate_cable_bend_force_hessian_avbd( - q_wp, - q_wc, - q_wp_rest, - q_wc_rest, - q_wp_prev, - q_wc_prev, + c_start = joint_constraint_start[joint_index] + + if jt == JointType.CABLE: + stretch_penalty_k = joint_penalty_k[c_start] + bend_penalty_k = joint_penalty_k[c_start + 1] + stretch_damping = joint_penalty_kd[c_start] + bend_damping = joint_penalty_kd[c_start + 1] + + total_force = wp.vec3(0.0) + total_torque = wp.vec3(0.0) + total_H_ll = wp.mat33(0.0) + total_H_al = wp.mat33(0.0) + total_H_aa = wp.mat33(0.0) + + if bend_penalty_k > 0.0: + q_wp = wp.transform_get_rotation(X_wp) + q_wc = wp.transform_get_rotation(X_wc) + q_wp_rest = wp.transform_get_rotation(X_wp_rest) + q_wc_rest = wp.transform_get_rotation(X_wc_rest) + q_wp_prev = wp.transform_get_rotation(X_wp_prev) + q_wc_prev = wp.transform_get_rotation(X_wc_prev) + sigma0 = joint_sigma_start[joint_index] + C_fric = joint_C_fric[joint_index] + bend_torque, bend_H_aa = evaluate_angular_constraint_force_hessian_isotropic( + q_wp, + q_wc, + q_wp_rest, + q_wc_rest, + q_wp_prev, + q_wc_prev, + is_parent_body, + bend_penalty_k, # effective per-joint bend stiffness (pre-scaled for rods) + sigma0, + C_fric, + bend_damping, + dt, + ) + total_torque = total_torque + bend_torque + total_H_aa = total_H_aa + bend_H_aa + + if stretch_penalty_k > 0.0: + # Stretch: meters residual C = x_c - x_p, using effective per-joint linear stiffness [N/m]. + # For rods created by `ModelBuilder.add_rod()`, this stiffness is pre-scaled by dividing by segment length. + k_eff = stretch_penalty_k + f_s, t_s, Hll_s, Hal_s, Haa_s = evaluate_linear_constraint_force_hessian_isotropic( + X_wp, + X_wc, + X_wp_prev, + X_wc_prev, + parent_pose, + child_pose, + parent_com, + child_com, + is_parent_body, + k_eff, + stretch_damping, + dt, + ) + total_force = total_force + f_s + total_torque = total_torque + t_s + total_H_ll = total_H_ll + Hll_s + total_H_al = total_H_al + Hal_s + total_H_aa = total_H_aa + Haa_s + + return total_force, total_torque, total_H_ll, total_H_al, total_H_aa + + elif jt == JointType.BALL: + # BALL: isotropic linear anchor-coincidence stored as a single scalar. + k = joint_penalty_k[c_start] + damping = joint_penalty_kd[c_start] + return evaluate_linear_constraint_force_hessian_isotropic( + X_wp, + X_wc, + X_wp_prev, + X_wc_prev, + parent_pose, + child_pose, + parent_com, + child_com, is_parent_body, - bend_k_eff, - sigma0, - C_fric, - bend_damping, + k, + damping, dt, ) - total_torque = total_torque + bend_torque - total_H_aa = total_H_aa + bend_H_aa - if stretch_penalty_k > 0.0: - f_s, t_s, Hll_s, Hal_s, Haa_s = evaluate_cable_stretch_force_hessian_avbd( + elif jt == JointType.FIXED: + # FIXED: isotropic linear anchor-coincidence + isotropic angular constraint. + # Linear part + k_lin = joint_penalty_k[c_start + 0] + kd_lin = joint_penalty_kd[c_start + 0] + f_lin, t_lin, Hll_lin, Hal_lin, Haa_lin = evaluate_linear_constraint_force_hessian_isotropic( X_wp, X_wc, X_wp_prev, @@ -954,17 +1009,45 @@ def evaluate_joint_force_hessian( parent_com, child_com, is_parent_body, - stretch_penalty_k, - stretch_damping, + k_lin, + kd_lin, dt, ) - total_force = total_force + f_s - total_torque = total_torque + t_s - total_H_ll = total_H_ll + Hll_s - total_H_al = total_H_al + Hal_s - total_H_aa = total_H_aa + Haa_s - return total_force, total_torque, total_H_ll, total_H_al, total_H_aa + # Rotational part: reuse kappa-based isotropic rotational constraint. + k_ang = joint_penalty_k[c_start + 1] + kd_ang = joint_penalty_kd[c_start + 1] + if k_ang > 0.0: + q_wp = wp.transform_get_rotation(X_wp) + q_wc = wp.transform_get_rotation(X_wc) + q_wp_rest = wp.transform_get_rotation(X_wp_rest) + q_wc_rest = wp.transform_get_rotation(X_wc_rest) + q_wp_prev = wp.transform_get_rotation(X_wp_prev) + q_wc_prev = wp.transform_get_rotation(X_wc_prev) + sigma0 = wp.vec3(0.0) + C_fric = wp.vec3(0.0) + t_ang, Haa_ang = evaluate_angular_constraint_force_hessian_isotropic( + q_wp, + q_wc, + q_wp_rest, + q_wc_rest, + q_wp_prev, + q_wc_prev, + is_parent_body, + # Note: FIXED uses k_ang directly as an effective stiffness. + k_ang, + sigma0, + C_fric, + kd_ang, + dt, + ) + else: + t_ang = wp.vec3(0.0) + Haa_ang = wp.mat33(0.0) + + return f_lin, t_lin + t_ang, Hll_lin, Hal_lin, Haa_lin + Haa_ang + + return wp.vec3(0.0), wp.vec3(0.0), wp.mat33(0.0), wp.mat33(0.0), wp.mat33(0.0) # ----------------------------- @@ -1187,23 +1270,23 @@ def build_body_particle_contact_lists( @wp.kernel def warmstart_joints( # Inputs - joint_target_ke: wp.array(dtype=float), + joint_penalty_k_max: wp.array(dtype=float), joint_penalty_k_min: wp.array(dtype=float), gamma: float, # Input/output joint_penalty_k: wp.array(dtype=float), ): """ - Warm-start per-DOF penalty stiffness for joint constraints (runs once per step). + Warm-start per-scalar-constraint penalty stiffness for rigid joint constraints (runs once per step). - Algorithm (per DOF): - - Decay previous penalty: k <- gamma * k - - Clamp to [joint_penalty_k_min[i], joint_target_ke[i]] + Algorithm (per constraint scalar i): + - Decay previous penalty: k <- gamma * k (typically gamma in [0, 1]) + - Clamp to [joint_penalty_k_min[i], joint_penalty_k_max[i]] so k_min <= k <= k_max always. """ i = wp.tid() k_prev = joint_penalty_k[i] - k_new = wp.clamp(gamma * k_prev, joint_penalty_k_min[i], joint_target_ke[i]) + k_new = wp.clamp(gamma * k_prev, joint_penalty_k_min[i], joint_penalty_k_max[i]) joint_penalty_k[i] = k_new @@ -1307,11 +1390,10 @@ def compute_cable_dahl_parameters( joint_child: wp.array(dtype=int), joint_X_p: wp.array(dtype=wp.transform), joint_X_c: wp.array(dtype=wp.transform), - joint_qd_start: wp.array(dtype=int), - joint_target_ke: wp.array(dtype=float), + joint_constraint_start: wp.array(dtype=int), + joint_penalty_k_max: wp.array(dtype=float), body_q: wp.array(dtype=wp.transform), body_q_rest: wp.array(dtype=wp.transform), - body_com: wp.array(dtype=wp.vec3), joint_sigma_prev: wp.array(dtype=wp.vec3), joint_kappa_prev: wp.array(dtype=wp.vec3), joint_dkappa_prev: wp.array(dtype=wp.vec3), @@ -1370,24 +1452,13 @@ def compute_cable_dahl_parameters( eps_max = joint_eps_max[j] tau = joint_tau[j] - # Get bend stiffness and segment length for this joint - dof_start = joint_qd_start[j] - bend_dof_index = dof_start + 1 - k_bend_material = joint_target_ke[bend_dof_index] + # Use the per-joint bend stiffness target (cap) from the solver constraint caps (constraint slot 1 for cables). + c_start = joint_constraint_start[j] + k_bend_target = joint_penalty_k_max[c_start + 1] - # Compute segment length (Cosserat rod discretization) - parent_com = body_com[parent] - rest_length = 2.0 * parent_com[2] + # Friction envelope: sigma_max = k_bend_target * eps_max. - # Skip degenerate segments - if rest_length < _SMALL_LENGTH_EPS: - joint_sigma_start[j] = wp.vec3(0.0) - joint_C_fric[j] = wp.vec3(0.0) - return - - # Effective stiffness and friction envelope - k_bend = k_bend_material / rest_length - sigma_max = k_bend * eps_max + sigma_max = k_bend_target * eps_max if sigma_max <= 0.0 or tau <= 0.0: joint_sigma_start[j] = wp.vec3(0.0) joint_C_fric[j] = wp.vec3(0.0) @@ -1748,10 +1819,10 @@ def solve_rigid_body( joint_child: wp.array(dtype=int), joint_X_p: wp.array(dtype=wp.transform), joint_X_c: wp.array(dtype=wp.transform), - joint_qd_start: wp.array(dtype=int), - joint_target_kd: wp.array(dtype=float), - # AVBD per-DOF penalty state + joint_constraint_start: wp.array(dtype=int), + # AVBD per-constraint penalty state (scalar constraints indexed via joint_constraint_start) joint_penalty_k: wp.array(dtype=float), + joint_penalty_kd: wp.array(dtype=float), # Dahl hysteresis parameters (frozen for this timestep, component-wise vec3 per joint) joint_sigma_start: wp.array(dtype=wp.vec3), joint_C_fric: wp.array(dtype=wp.vec3), @@ -1767,7 +1838,12 @@ def solve_rigid_body( AVBD solve step for rigid bodies using block Cholesky decomposition. Solves the 6-DOF rigid body system by assembling inertial, joint, and collision - contributions into a 6x6 block system [M, C; C^T, A] and solving via Schur complement. + contributions into a 6x6 block system: + + [ H_ll H_al^T ] + [ H_al H_aa ] + + and solving via Schur complement. Consistent with VBD particle solve pattern: inertia + external + constraint forces. Algorithm: @@ -1789,7 +1865,7 @@ def solve_rigid_body( body_com: Center of mass offsets (local body frame). adjacency: Body-joint adjacency (CSR format). joint_*: Joint configuration arrays. - joint_penalty_k: AVBD per-DOF penalty stiffness. + joint_penalty_k: AVBD per-constraint penalty stiffness (one scalar per solver constraint component). joint_sigma_start: Dahl hysteresis state at start of step. joint_C_fric: Dahl friction configuration per joint. external_forces: External linear forces from rigid contacts. @@ -1797,7 +1873,8 @@ def solve_rigid_body( external_hessian_ll: Linear-linear Hessian block (3x3) from rigid contacts. external_hessian_al: Angular-linear coupling Hessian block (3x3) from rigid contacts. external_hessian_aa: Angular-angular Hessian block (3x3) from rigid contacts. - body_q: Current body transforms (input/output, updated in-place). + body_q: Current body transforms (input). + body_q_new: Updated body transforms (output) for the current solve sweep. Note: - All forces, torques, and Hessian blocks are expressed in the world frame. @@ -1898,9 +1975,9 @@ def solve_rigid_body( joint_child, joint_X_p, joint_X_c, - joint_qd_start, - joint_target_kd, + joint_constraint_start, joint_penalty_k, + joint_penalty_kd, joint_sigma_start, joint_C_fric, dt, @@ -1927,7 +2004,7 @@ def solve_rigid_body( # Compute M^-1 * f_force MinvF = chol33_solve(Lm_p, f_force) - # Compute M^-1 * C^T + # Compute H_ll^{-1} * (H_al^T) C_r0 = wp.vec3(h_al[0, 0], h_al[0, 1], h_al[0, 2]) C_r1 = wp.vec3(h_al[1, 0], h_al[1, 1], h_al[1, 2]) C_r2 = wp.vec3(h_al[2, 0], h_al[2, 1], h_al[2, 2]) @@ -1936,6 +2013,7 @@ def solve_rigid_body( X1 = chol33_solve(Lm_p, C_r1) X2 = chol33_solve(Lm_p, C_r2) + # Columns are the solved vectors X0, X1, X2 MinvCt = wp.mat33(X0[0], X1[0], X2[0], X0[1], X1[1], X2[1], X0[2], X1[2], X2[2]) # Compute Schur complement @@ -2007,20 +2085,23 @@ def update_duals_joint( joint_child: wp.array(dtype=int), joint_X_p: wp.array(dtype=wp.transform), joint_X_c: wp.array(dtype=wp.transform), - joint_qd_start: wp.array(dtype=int), - joint_dof_dim: wp.array(dtype=int, ndim=2), - joint_target_ke: wp.array(dtype=float), + joint_constraint_start: wp.array(dtype=int), + joint_penalty_k_max: wp.array(dtype=float), body_q: wp.array(dtype=wp.transform), body_q_rest: wp.array(dtype=wp.transform), - body_com: wp.array(dtype=wp.vec3), beta: float, joint_penalty_k: wp.array(dtype=float), # input/output ): """ Update AVBD penalty parameters for joint constraints (per-iteration). - Increases per-DOF penalties based on current constraint violation magnitudes, - clamped by the per-DOF material target stiffness. + Increases per-constraint penalties based on current constraint violation magnitudes, + clamped by the per-constraint stiffness cap ``joint_penalty_k_max``. + + Notes: + - For ``JointType.CABLE``, ``joint_penalty_k_max`` is populated from ``model.joint_target_ke`` (material/constraint tuning). + - For rigid constraints like ``JointType.BALL``, ``joint_penalty_k_max`` is populated from solver-level caps + (e.g. ``rigid_joint_linear_ke``). Args: joint_type: Joint types @@ -2028,14 +2109,12 @@ def update_duals_joint( joint_child: Child body indices joint_X_p: Parent joint frames (local) joint_X_c: Child joint frames (local) - joint_qd_start: Start DOF index per joint - joint_dof_dim: [num_lin, num_ang] DOF per joint - joint_target_ke: Target stiffness per DOF + joint_constraint_start: Start index per joint in the solver constraint layout + joint_penalty_k_max: Per-constraint stiffness cap (used for penalty clamping) body_q: Current body transforms (world) body_q_rest: Rest body transforms (world) - body_com: Body COM offsets (local) used to infer segment length beta: Adaptation rate - joint_penalty_k: In/out per-DOF adaptive penalties + joint_penalty_k: In/out per-constraint adaptive penalties """ j = wp.tid() parent = joint_parent[j] @@ -2045,9 +2124,12 @@ def update_duals_joint( if parent < 0 or child < 0: return - # Read DOF configuration - dof_start = joint_qd_start[j] - lin_axes = joint_dof_dim[j, 0] + jt = joint_type[j] + if jt != JointType.CABLE and jt != JointType.BALL and jt != JointType.FIXED: + return + + # Read solver constraint start index + c_start = joint_constraint_start[j] # Compute joint frames in world space X_wp = body_q[parent] * joint_X_p[j] @@ -2056,23 +2138,15 @@ def update_duals_joint( X_wc_rest = body_q_rest[child] * joint_X_c[j] # Cable joint: adaptive penalty for stretch and bend constraints - if joint_type[j] == JointType.CABLE: + if jt == JointType.CABLE: # Read joint frame rotations q_wp = wp.transform_get_rotation(X_wp) q_wc = wp.transform_get_rotation(X_wc) q_wp_rest = wp.transform_get_rotation(X_wp_rest) q_wc_rest = wp.transform_get_rotation(X_wc_rest) - # Compute segment rest length (capsule length) - shared by both modes - parent_com_local = body_com[parent] - rest_length = 2.0 * parent_com_local[2] - - # Skip degenerate segments - if rest_length < _SMALL_LENGTH_EPS: - return - - # Compute stretch constraint violation (simple pinning) - # C = ||x_c - x_p|| (distance between attachment points) + # Compute scalar violation magnitudes used for penalty growth. + # Stretch uses meters residual magnitude ||x_c - x_p|| to update an effective [N/m] stiffness. x_p = wp.transform_get_translation(X_wp) x_c = wp.transform_get_translation(X_wc) C_stretch = wp.length(x_c - x_p) @@ -2081,19 +2155,56 @@ def update_duals_joint( kappa = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest) C_bend = wp.length(kappa) - # Update stretch penalty (DOF 0) - stretch_idx = dof_start - stiffness_stretch = joint_target_ke[stretch_idx] + # Update stretch penalty (constraint slot 0) + stretch_idx = c_start + stiffness_stretch = joint_penalty_k_max[stretch_idx] k_stretch = joint_penalty_k[stretch_idx] k_stretch_new = wp.min(k_stretch + beta * C_stretch, stiffness_stretch) joint_penalty_k[stretch_idx] = k_stretch_new - # Update bend penalty (DOF 1) - bend_idx = dof_start + lin_axes - stiffness_bend = joint_target_ke[bend_idx] + # Update bend penalty (constraint slot 1) + bend_idx = c_start + 1 + stiffness_bend = joint_penalty_k_max[bend_idx] k_bend = joint_penalty_k[bend_idx] k_bend_new = wp.min(k_bend + beta * C_bend, stiffness_bend) joint_penalty_k[bend_idx] = k_bend_new + return + + # BALL joint: update isotropic linear anchor-coincidence penalty (single scalar). + if jt == JointType.BALL: + # Scalar violation magnitude used for penalty growth; force/Hessian uses C_vec directly. + x_p = wp.transform_get_translation(X_wp) + x_c = wp.transform_get_translation(X_wc) + C_vec = x_c - x_p + C_lin = wp.length(C_vec) + + i0 = c_start + k0 = joint_penalty_k[i0] + joint_penalty_k[i0] = wp.min(k0 + beta * C_lin, joint_penalty_k_max[i0]) + return + + # FIXED joint: update isotropic linear + isotropic angular penalties (2 scalars). + if jt == JointType.FIXED: + i_lin = c_start + 0 + i_ang = c_start + 1 + + # Linear violation magnitude + x_p = wp.transform_get_translation(X_wp) + x_c = wp.transform_get_translation(X_wc) + C_lin = wp.length(x_c - x_p) + k_lin = joint_penalty_k[i_lin] + joint_penalty_k[i_lin] = wp.min(k_lin + beta * C_lin, joint_penalty_k_max[i_lin]) + + # Angular violation magnitude from kappa + q_wp = wp.transform_get_rotation(X_wp) + q_wc = wp.transform_get_rotation(X_wc) + q_wp_rest = wp.transform_get_rotation(X_wp_rest) + q_wc_rest = wp.transform_get_rotation(X_wc_rest) + kappa = cable_get_kappa(q_wp, q_wc, q_wp_rest, q_wc_rest) + C_ang = wp.length(kappa) + k_ang = joint_penalty_k[i_ang] + joint_penalty_k[i_ang] = wp.min(k_ang + beta * C_ang, joint_penalty_k_max[i_ang]) + return @wp.kernel @@ -2302,12 +2413,11 @@ def update_cable_dahl_state( joint_child: wp.array(dtype=int), joint_X_p: wp.array(dtype=wp.transform), joint_X_c: wp.array(dtype=wp.transform), - joint_qd_start: wp.array(dtype=int), - joint_target_ke: wp.array(dtype=float), + joint_constraint_start: wp.array(dtype=int), + joint_penalty_k_max: wp.array(dtype=float), # Body states (final, after solver convergence) body_q: wp.array(dtype=wp.transform), body_q_rest: wp.array(dtype=wp.transform), - body_com: wp.array(dtype=wp.vec3), # Dahl model parameters (PER-JOINT arrays, isotropic) joint_eps_max: wp.array(dtype=float), joint_tau: wp.array(dtype=float), @@ -2326,11 +2436,10 @@ def update_cable_dahl_state( joint_type: Joint type (only updates for cable joints) joint_parent, joint_child: Parent/child body indices joint_X_p, joint_X_c: Joint frames in parent/child - joint_qd_start: DOF start index per joint - joint_target_ke: Target stiffness per DOF [N*m^2] (material EI for bending) + joint_constraint_start: Start index per joint in the solver constraint layout + joint_penalty_k_max: Per-constraint stiffness cap; for cables, bend cap stores effective per-joint bend stiffness [N*m] body_q: Final body transforms (after convergence) body_q_rest: Rest body transforms - body_com: Body COM in local frame (used to compute segment length for Cosserat discretization) joint_sigma_prev: Friction stress state (read old, write new), wp.vec3 per joint joint_kappa_prev: Curvature state (read old, write new), wp.vec3 per joint joint_dkappa_prev: Delta-kappa state (write new), wp.vec3 per joint @@ -2374,27 +2483,12 @@ def update_cable_dahl_state( eps_max = joint_eps_max[j] # Maximum persistent strain [rad] tau = joint_tau[j] # Memory decay length [rad] - # Get bend stiffness and segment length for this joint - # Cable joint DOFs: DOF 0 = stretch (linear), DOF 1 = bend (angular) - dof_start = joint_qd_start[j] - bend_dof_index = dof_start + 1 - k_bend_material = joint_target_ke[bend_dof_index] # Material stiffness EI [N*m^2] - - # Compute segment length (Cosserat rod discretization) - parent_com = body_com[parent] - rest_length = 2.0 * parent_com[2] # Capsule segment length [m] - - # Skip degenerate segments - if rest_length < _SMALL_LENGTH_EPS: - return - - # Effective stiffness (same scaling as elastic force): k_eff = EI / L - k_bend = k_bend_material / rest_length # [N*m^2] / [m] = [N*m] + # Bend stiffness target (cap) is stored in constraint slot 1 for cable joints. + c_start = joint_constraint_start[j] + k_bend_target = joint_penalty_k_max[c_start + 1] # [N*m] - # Compute maximum friction stress from strain (paper's approach: sigma_max = k_eff * eps_max) - # Note: Must use k_eff (not k_material) to match elastic force scaling. - # For isotropic case: same sigma_max for all components - sigma_max = k_bend * eps_max # [N*m] * [rad] = [N*m] + # Friction envelope: sigma_max = k_bend_target * eps_max. + sigma_max = k_bend_target * eps_max # [N*m] # Early-out: disable friction if envelope is zero/invalid if sigma_max <= 0.0 or tau <= 0.0: diff --git a/newton/_src/solvers/vbd/solver_vbd.py b/newton/_src/solvers/vbd/solver_vbd.py index 8682b57a8c..ed978b0089 100644 --- a/newton/_src/solvers/vbd/solver_vbd.py +++ b/newton/_src/solvers/vbd/solver_vbd.py @@ -16,12 +16,22 @@ from __future__ import annotations import warnings +from typing import Any import numpy as np import warp as wp from ...core.types import override -from ...sim import Contacts, Control, JointType, Model, State +from ...sim import ( + Contacts, + Control, + JointType, + Model, + ModelAttributeAssignment, + ModelAttributeFrequency, + ModelBuilder, + State, +) from ..solver import SolverBase from .particle_vbd_kernels import ( NUM_THREADS_PER_COLLISION_PRIMITIVE, @@ -157,13 +167,15 @@ def __init__( rigid_avbd_beta: float = 1.0e5, rigid_avbd_gamma: float = 0.99, rigid_contact_k_start: float = 1.0e2, # AVBD: initial stiffness for all body contacts (body-body + body-particle) - rigid_joint_linear_k_start: float = 1.0e4, # AVBD: initial stiffness seed for linear joint DOFs (e.g., cable stretch) - rigid_joint_angular_k_start: float = 1.0e1, # AVBD: initial stiffness seed for angular joint DOFs (e.g., cable bend) + rigid_joint_linear_k_start: float = 1.0e4, # AVBD: initial stiffness seed for linear joint constraints + rigid_joint_angular_k_start: float = 1.0e1, # AVBD: initial stiffness seed for angular joint constraints + rigid_joint_linear_ke: float = 1.0e9, # AVBD: stiffness cap for non-cable linear joint constraints (BALL/FIXED/etc.) + rigid_joint_angular_ke: float = 1.0e9, # AVBD: stiffness cap for non-cable angular joint constraints (FIXED/etc.) + rigid_joint_linear_kd: float = 0.0, # AVBD: Rayleigh damping coefficient for non-cable linear joint constraints + rigid_joint_angular_kd: float = 0.0, # AVBD: Rayleigh damping coefficient for non-cable angular joint constraints rigid_body_contact_buffer_size: int = 64, rigid_body_particle_contact_buffer_size: int = 256, rigid_enable_dahl_friction: bool = False, # Cable bending plasticity/hysteresis - rigid_dahl_eps_max: float | wp.array = 0.5, # Dahl: max persistent strain - rigid_dahl_tau: float | wp.array = 1.0, # Dahl: memory decay length ): """ Args: @@ -216,19 +228,24 @@ def __init__( rigid_avbd_gamma: Warmstart decay for penalty k (cross-step decay factor for rigid body constraints). rigid_contact_k_start: Initial penalty stiffness for all body contact constraints, including both body-body (rigid-rigid) and body-particle (rigid-particle) contacts (AVBD). - rigid_joint_linear_k_start: Initial penalty seed for linear joint DOFs (e.g., cable stretch). Used to seed the per-DOF - adaptive penalties for all linear joint constraints. - rigid_joint_angular_k_start: Initial penalty seed for angular joint DOFs (e.g., cable bend). Used to seed the per-DOF - adaptive penalties for all angular joint constraints. + rigid_joint_linear_k_start: Initial penalty seed for linear joint constraints (e.g., cable stretch, BALL linear). + Used to seed the per-constraint adaptive penalties for all linear joint constraints. + rigid_joint_angular_k_start: Initial penalty seed for angular joint constraints (e.g., cable bend, FIXED angular). + Used to seed the per-constraint adaptive penalties for all angular joint constraints. + rigid_joint_linear_ke: Stiffness cap used by AVBD for **non-cable** linear joint constraint scalars + (e.g., BALL and the linear part of FIXED). Cable joints use the per-joint caps in + ``model.joint_target_ke`` instead (cable interprets ``joint_target_ke/kd`` as constraint tuning). + rigid_joint_angular_ke: Stiffness cap used by AVBD for **non-cable** angular joint constraint scalars + (e.g., FIXED). + rigid_joint_linear_kd: Rayleigh damping coefficient for non-cable linear joint constraints (paired with + ``rigid_joint_linear_ke``). + rigid_joint_angular_kd: Rayleigh damping coefficient for non-cable angular joint constraints (paired with + ``rigid_joint_angular_ke``). rigid_body_contact_buffer_size: Max body-body (rigid-rigid) contacts per rigid body for per-body contact lists (tune based on expected body-body contact density). rigid_body_particle_contact_buffer_size: Max body-particle (rigid-particle) contacts per rigid body for per-body soft-contact lists (tune based on expected body-particle contact density). rigid_enable_dahl_friction: Enable Dahl hysteresis friction model for cable bending (default: False). - rigid_dahl_eps_max: Maximum persistent strain (curvature) [rad] for Dahl friction model. Can be: - - float: Same value for all joints - - array: Per-joint values for heterogeneous cables - rigid_dahl_tau: Memory decay length [rad] for Dahl friction model. Controls plasticity. Can be: - - float: Same value for all joints - - array: Per-joint values + Configure per-joint Dahl parameters via the solver-registered custom model attributes + ``model.vbd.dahl_eps_max`` and ``model.vbd.dahl_tau``. Note: - The `integrate_with_external_rigid_solver` argument enables one-way coupling between rigid body and soft body @@ -279,11 +296,13 @@ def __init__( rigid_contact_k_start, rigid_joint_linear_k_start, rigid_joint_angular_k_start, + rigid_joint_linear_ke, + rigid_joint_angular_ke, + rigid_joint_linear_kd, + rigid_joint_angular_kd, rigid_body_contact_buffer_size, rigid_body_particle_contact_buffer_size, rigid_enable_dahl_friction, - rigid_dahl_eps_max, - rigid_dahl_tau, ) # Rigid-only flag to control whether to update cross-step history @@ -403,11 +422,13 @@ def _init_rigid_system( rigid_contact_k_start: float, rigid_joint_linear_k_start: float, rigid_joint_angular_k_start: float, + rigid_joint_linear_ke: float, + rigid_joint_angular_ke: float, + rigid_joint_linear_kd: float, + rigid_joint_angular_kd: float, rigid_body_contact_buffer_size: int, rigid_body_particle_contact_buffer_size: int, rigid_enable_dahl_friction: bool, - rigid_dahl_eps_max: float | wp.array, - rigid_dahl_tau: float | wp.array, ): """Initialize rigid body-specific AVBD data structures and settings. @@ -422,6 +443,12 @@ def _init_rigid_system( # Common initial penalty seed / lower bound for body contacts (clamped to non-negative) self.k_start_body_contact = max(0.0, rigid_contact_k_start) + # Joint constraint caps and damping for non-cable joints (constraint enforcement, not drives) + self.rigid_joint_linear_ke = max(0.0, rigid_joint_linear_ke) + self.rigid_joint_angular_ke = max(0.0, rigid_joint_angular_ke) + self.rigid_joint_linear_kd = max(0.0, rigid_joint_linear_kd) + self.rigid_joint_angular_kd = max(0.0, rigid_joint_angular_kd) + # ------------------------------------------------------------- # Rigid-only AVBD state (used when SolverVBD integrates bodies) # ------------------------------------------------------------- @@ -462,7 +489,8 @@ def _init_rigid_system( ) # AVBD constraint penalties - # Joint penalties (per-DOF adaptive penalties seeded from joint-wide linear/angular stiffness) + # Joint constraint layout + penalties (solver constraint scalars) + self._init_joint_constraint_layout() self.joint_penalty_k = self._init_joint_penalty_k(rigid_joint_linear_k_start, rigid_joint_angular_k_start) # Contact penalties (adaptive penalties for body-body contacts) @@ -501,7 +529,14 @@ def _init_rigid_system( if model.joint_count == 0: self.enable_dahl_friction = False else: - self._init_dahl_params(rigid_dahl_eps_max, rigid_dahl_tau, model) + # Read per-joint Dahl parameters from model.vbd if present; otherwise use defaults (eps_max=0.5, tau=1.0). + # Recommended: call SolverVBD.register_custom_attributes(builder) before finalize() to allocate these arrays. + vbd_attrs: Any = getattr(model, "vbd", None) + if vbd_attrs is not None and hasattr(vbd_attrs, "dahl_eps_max") and hasattr(vbd_attrs, "dahl_tau"): + self.joint_dahl_eps_max = vbd_attrs.dahl_eps_max + self.joint_dahl_tau = vbd_attrs.dahl_tau + else: + self._init_dahl_params(0.5, 1.0, model) # ------------------------------------------------------------- # Body-particle interaction - shared state @@ -535,41 +570,190 @@ def _init_rigid_system( # Initialization Helper Methods # ===================================================== + def _init_joint_constraint_layout(self) -> None: + """Initialize VBD-owned joint constraint indexing. + + VBD stores and adapts penalty stiffness values for *scalar constraint components*: + - ``JointType.CABLE``: 2 scalars (stretch/linear, bend/angular) + - ``JointType.BALL``: 1 scalar (isotropic linear anchor-coincidence) + - ``JointType.FIXED``: 2 scalars (isotropic linear anchor-coincidence + isotropic angular) + - ``JointType.FREE``: 0 scalars (not a constraint) + + Ordering (must match kernel indexing via ``joint_constraint_start``): + - ``JointType.CABLE``: [stretch (linear), bend (angular)] + - ``JointType.BALL``: [linear] + - ``JointType.FIXED``: [linear, angular] + + Any other joint type will raise ``NotImplementedError``. + """ + n_j = self.model.joint_count + with wp.ScopedDevice("cpu"): + jt_cpu = self.model.joint_type.to("cpu") + jt = jt_cpu.numpy() if hasattr(jt_cpu, "numpy") else np.asarray(jt_cpu, dtype=int) + + dim_np = np.zeros((n_j,), dtype=np.int32) + for j in range(n_j): + if jt[j] == JointType.CABLE: + dim_np[j] = 2 + elif jt[j] == JointType.BALL: + dim_np[j] = 1 + elif jt[j] == JointType.FIXED: + dim_np[j] = 2 + else: + if jt[j] != JointType.FREE: + raise NotImplementedError( + f"SolverVBD rigid joints: JointType.{JointType(jt[j]).name} is not implemented yet " + "(only CABLE, BALL, and FIXED are supported)." + ) + dim_np[j] = 0 + + start_np = np.zeros((n_j,), dtype=np.int32) + c = 0 + for j in range(n_j): + start_np[j] = np.int32(c) + c += int(dim_np[j]) + + self.joint_constraint_count = int(c) + self.joint_constraint_dim = wp.array(dim_np, dtype=wp.int32, device=self.device) + self.joint_constraint_start = wp.array(start_np, dtype=wp.int32, device=self.device) + def _init_joint_penalty_k(self, k_start_joint_linear: float, k_start_joint_angular: float): """ - Build initial per-DOF joint penalty array on CPU and upload to solver device. - - Default seed is a global rigid-body penalty for all DOFs. - - Optionally override cable stretch/bend DOFs per joint. + Build initial joint penalty state on CPU and upload to solver device. + + This initializes the solver-owned joint constraint parameter arrays used by VBD. + The arrays are sized by ``self.joint_constraint_count`` and indexed using + ``self.joint_constraint_start`` (solver constraint indexing), not by model DOF indexing. + + Arrays: + - ``k0``: initial penalty stiffness for each solver constraint scalar (stored as ``self.joint_penalty_k``) + - ``k_min``: warmstart floor for each solver constraint scalar (stored as ``self.joint_penalty_k_min``) + - ``k_max``: stiffness cap for each solver constraint scalar (stored as ``self.joint_penalty_k_max``) + - ``kd``: damping coefficient for each solver constraint scalar (stored as ``self.joint_penalty_kd``) + + Supported rigid joint constraint types in SolverVBD: + - ``JointType.CABLE`` (2 scalars: stretch + bend) + - ``JointType.BALL`` (1 scalar: isotropic linear anchor-coincidence) + - ``JointType.FIXED`` (2 scalars: isotropic linear anchor-coincidence + isotropic angular) + + ``JointType.FREE`` joints (created by :meth:`ModelBuilder.add_body`) are not constraints and are ignored. + Any other joint types will raise ``NotImplementedError``. """ - dof_count = self.model.joint_dof_count + if ( + not hasattr(self, "joint_constraint_start") + or not hasattr(self, "joint_constraint_dim") + or not hasattr(self, "joint_constraint_count") + ): + raise RuntimeError( + "SolverVBD joint constraint layout is not initialized. " + "Call SolverVBD._init_joint_constraint_layout() before _init_joint_penalty_k()." + ) + + if self.joint_constraint_count < 0: + raise RuntimeError( + f"SolverVBD joint constraint layout is invalid: joint_constraint_count={self.joint_constraint_count!r}" + ) + + constraint_count = self.joint_constraint_count with wp.ScopedDevice("cpu"): - # Seed all DOFs with the joint-linear stiffness and specialize cable bend DOFs below. - # This keeps a single pair of joint-wide seeds as the authoritative source of joint stiffness. + # Per-constraint AVBD penalty state: + # - k0: initial penalty stiffness for this scalar constraint + # - k_min: warmstart floor (so k doesn't decay below this across steps) + # - k_max: stiffness cap (so k never exceeds the chosen target for this constraint) + # + # We start from solver-level seeds (k_start_*), but clamp to the per-constraint cap (k_max) so we always + # satisfy k_min <= k0 <= k_max. stretch_k = max(0.0, k_start_joint_linear) - joint_k_min_np = np.full((dof_count,), 0.0, dtype=float) - joint_k0_np = np.full((dof_count,), stretch_k, dtype=float) + bend_k = max(0.0, k_start_joint_angular) + joint_k_min_np = np.zeros((constraint_count,), dtype=float) + joint_k0_np = np.zeros((constraint_count,), dtype=float) + # Per-constraint stiffness caps used for AVBD warmstart clamping and penalty growth limiting. + # - Cable constraints: use model.joint_target_ke (cable material/constraint tuning; still model-DOF indexed) + # - Ball constraints: use solver-level caps (rigid_joint_linear_ke) + # Start from zeros and explicitly fill per joint/constraint-slot below for clarity. + joint_k_max_np = np.zeros((constraint_count,), dtype=float) + joint_kd_np = np.zeros((constraint_count,), dtype=float) jt_cpu = self.model.joint_type.to("cpu") jdofs_cpu = self.model.joint_qd_start.to("cpu") + jtarget_ke_cpu = self.model.joint_target_ke.to("cpu") + jtarget_kd_cpu = self.model.joint_target_kd.to("cpu") + jc_start_cpu = self.joint_constraint_start.to("cpu") jt = jt_cpu.numpy() if hasattr(jt_cpu, "numpy") else np.asarray(jt_cpu, dtype=int) jdofs = jdofs_cpu.numpy() if hasattr(jdofs_cpu, "numpy") else np.asarray(jdofs_cpu, dtype=int) + jc_start = ( + jc_start_cpu.numpy() if hasattr(jc_start_cpu, "numpy") else np.asarray(jc_start_cpu, dtype=np.int32) + ) + jtarget_ke = ( + jtarget_ke_cpu.numpy() if hasattr(jtarget_ke_cpu, "numpy") else np.asarray(jtarget_ke_cpu, dtype=float) + ) + jtarget_kd = ( + jtarget_kd_cpu.numpy() if hasattr(jtarget_kd_cpu, "numpy") else np.asarray(jtarget_kd_cpu, dtype=float) + ) n_j = self.model.joint_count - bend_k = max(0.0, k_start_joint_angular) for j in range(n_j): if jt[j] == JointType.CABLE: - dof0 = jdofs[j] - # DOF 0: cable stretch; DOF 1: cable bend - joint_k0_np[dof0] = stretch_k - joint_k0_np[dof0 + 1] = bend_k - # Per-DOF lower bounds: use k_start_* for cable stretch/bend, 0 otherwise - joint_k_min_np[dof0] = stretch_k - joint_k_min_np[dof0 + 1] = bend_k - - # Upload to device: initial penalties and per-DOF lower bounds - joint_penalty_k_min = wp.array(joint_k_min_np, dtype=float, device=self.device) - self.joint_penalty_k_min = joint_penalty_k_min + c0 = int(jc_start[j]) + dof0 = int(jdofs[j]) + # CABLE requires 2 DOF entries in model.joint_target_ke/kd starting at joint_qd_start[j]. + if dof0 < 0 or (dof0 + 1) >= len(jtarget_ke) or (dof0 + 1) >= len(jtarget_kd): + raise RuntimeError( + "SolverVBD _init_joint_penalty_k: JointType.CABLE requires 2 DOF entries in " + "model.joint_target_ke/kd starting at joint_qd_start[j]. " + f"Got joint_index={j}, joint_qd_start={dof0}, " + f"len(joint_target_ke)={len(jtarget_ke)}, len(joint_target_kd)={len(jtarget_kd)}." + ) + # Constraint 0: cable stretch; constraint 1: cable bend + # Caps come from model.joint_target_ke (still model DOF indexed for cable material tuning). + joint_k_max_np[c0] = jtarget_ke[dof0] + joint_k_max_np[c0 + 1] = jtarget_ke[dof0 + 1] + # Per-slot warmstart lower bounds: + # - Use k_start_* as the floor, but clamp to the cap so k_min <= k_max always. + joint_k_min_np[c0] = min(stretch_k, joint_k_max_np[c0]) + joint_k_min_np[c0 + 1] = min(bend_k, joint_k_max_np[c0 + 1]) + # Initial seed: clamp to cap so k0 <= k_max + joint_k0_np[c0] = min(stretch_k, joint_k_max_np[c0]) + joint_k0_np[c0 + 1] = min(bend_k, joint_k_max_np[c0 + 1]) + # Damping comes from model.joint_target_kd (still model DOF indexed for cable tuning). + joint_kd_np[c0] = jtarget_kd[dof0] + joint_kd_np[c0 + 1] = jtarget_kd[dof0 + 1] + elif jt[j] == JointType.BALL: + # BALL joints: isotropic linear anchor-coincidence constraint stored as a single scalar. + c0 = int(jc_start[j]) + joint_k_max_np[c0] = self.rigid_joint_linear_ke + k_floor = min(stretch_k, self.rigid_joint_linear_ke) + joint_k_min_np[c0] = k_floor + joint_k0_np[c0] = k_floor + joint_kd_np[c0] = self.rigid_joint_linear_kd + elif jt[j] == JointType.FIXED: + # FIXED joints are enforced as: + # - 1 isotropic linear anchor-coincidence constraint (vector error, scalar penalty) + # - 1 isotropic angular constraint (rotation-vector error, scalar penalty) + c0 = int(jc_start[j]) + + # Linear cap + floor (isotropic) + joint_k_max_np[c0 + 0] = self.rigid_joint_linear_ke + k_lin_floor = min(stretch_k, self.rigid_joint_linear_ke) + joint_k_min_np[c0 + 0] = k_lin_floor + joint_k0_np[c0 + 0] = k_lin_floor + joint_kd_np[c0 + 0] = self.rigid_joint_linear_kd + + # Angular cap + floor (isotropic) + joint_k_max_np[c0 + 1] = self.rigid_joint_angular_ke + k_ang_floor = min(bend_k, self.rigid_joint_angular_ke) + joint_k_min_np[c0 + 1] = k_ang_floor + joint_k0_np[c0 + 1] = k_ang_floor + joint_kd_np[c0 + 1] = self.rigid_joint_angular_kd + else: + # Layout builder already validated supported types; nothing to do for FREE. + pass + + # Upload to device: initial penalties, per-constraint caps, damping, and warmstart floors. + self.joint_penalty_k_min = wp.array(joint_k_min_np, dtype=float, device=self.device) + self.joint_penalty_k_max = wp.array(joint_k_max_np, dtype=float, device=self.device) + self.joint_penalty_kd = wp.array(joint_kd_np, dtype=float, device=self.device) return wp.array(joint_k0_np, dtype=float, device=self.device) def _init_dahl_params(self, eps_max_input, tau_input, model): @@ -620,6 +804,37 @@ def _init_dahl_params(self, eps_max_input, tau_input, model): # Direct host-to-device copy self.joint_dahl_tau = wp.array(tau_np, dtype=float, device=self.device) + @override + @classmethod + def register_custom_attributes(cls, builder: ModelBuilder) -> None: + """Register solver-specific custom Model attributes for SolverVBD. + + Currently used for cable bending plasticity/hysteresis (Dahl friction model). + + Attributes are declared in the ``vbd`` namespace so they can be authored in scenes + and in USD as ``newton:vbd:``. + """ + builder.add_custom_attribute( + ModelBuilder.CustomAttribute( + name="dahl_eps_max", + frequency=ModelAttributeFrequency.JOINT, + assignment=ModelAttributeAssignment.MODEL, + dtype=wp.float32, + default=0.5, + namespace="vbd", + ) + ) + builder.add_custom_attribute( + ModelBuilder.CustomAttribute( + name="dahl_tau", + frequency=ModelAttributeFrequency.JOINT, + assignment=ModelAttributeAssignment.MODEL, + dtype=wp.float32, + default=1.0, + namespace="vbd", + ) + ) + # ===================================================== # Adjacency Building Methods # ===================================================== @@ -882,7 +1097,7 @@ def step( state_in: State, state_out: State, control: Control, - contacts: Contacts, + contacts: Contacts | None, dt: float, ): """Execute one simulation timestep using VBD (particles) and AVBD (rigid bodies). @@ -900,7 +1115,9 @@ def step( state_in: Input state. state_out: Output state. control: Control inputs. - contacts: Collision contacts. + contacts: Contact data produced by :meth:`Model.collide` (rigid-rigid and rigid-particle contacts). + If None, rigid contact handling is skipped. Note that particle self-contact (if enabled) does not + depend on this argument. dt: Time step size. """ # Use and reset the rigid history update flag (warmstarts). @@ -970,13 +1187,16 @@ def initialize_particles(self, state_in: State, dt: float): def initialize_rigid_bodies( self, state_in: State, - contacts: Contacts, + contacts: Contacts | None, dt: float, update_rigid_history: bool, ): """Initialize rigid body states for AVBD solver (pre-iteration phase). - Performs forward integration, builds contact lists, and warmstarts AVBD penalty parameters. + Performs forward integration and initializes contact-related AVBD state when contacts are provided. + + If ``contacts`` is None, rigid contact-related work is skipped: + no per-body contact adjacency is built, and no contact penalties are warmstarted. """ model = self.model @@ -1008,63 +1228,65 @@ def initialize_rigid_bodies( ) if update_rigid_history: - # Use the Contacts buffer capacity as launch dimension - contact_launch_dim = contacts.rigid_contact_max + # Contact warmstarts / adjacency are optional: skip completely if contacts=None. + if contacts is not None: + # Use the Contacts buffer capacity as launch dimension + contact_launch_dim = contacts.rigid_contact_max - # Build per-body contact lists once per step - # Build body-body (rigid-rigid) contact lists - self.body_body_contact_counts.zero_() - wp.launch( - kernel=build_body_body_contact_lists, - dim=contact_launch_dim, - inputs=[ - contacts.rigid_contact_count, - contacts.rigid_contact_shape0, - contacts.rigid_contact_shape1, - model.shape_body, - self.body_body_contact_buffer_pre_alloc, - ], - outputs=[ - self.body_body_contact_counts, - self.body_body_contact_indices, - ], - device=self.device, - ) + # Build per-body contact lists once per step + # Build body-body (rigid-rigid) contact lists + self.body_body_contact_counts.zero_() + wp.launch( + kernel=build_body_body_contact_lists, + dim=contact_launch_dim, + inputs=[ + contacts.rigid_contact_count, + contacts.rigid_contact_shape0, + contacts.rigid_contact_shape1, + model.shape_body, + self.body_body_contact_buffer_pre_alloc, + ], + outputs=[ + self.body_body_contact_counts, + self.body_body_contact_indices, + ], + device=self.device, + ) - # Warmstart AVBD body-body contact penalties and pre-compute material properties - wp.launch( - kernel=warmstart_body_body_contacts, - inputs=[ - contacts.rigid_contact_count, - contacts.rigid_contact_shape0, - contacts.rigid_contact_shape1, - model.shape_material_ke, - model.shape_material_kd, - model.shape_material_mu, - self.k_start_body_contact, - ], - outputs=[ - self.body_body_contact_penalty_k, - self.body_body_contact_material_ke, - self.body_body_contact_material_kd, - self.body_body_contact_material_mu, - ], - dim=contact_launch_dim, - device=self.device, - ) + # Warmstart AVBD body-body contact penalties and pre-compute material properties + wp.launch( + kernel=warmstart_body_body_contacts, + inputs=[ + contacts.rigid_contact_count, + contacts.rigid_contact_shape0, + contacts.rigid_contact_shape1, + model.shape_material_ke, + model.shape_material_kd, + model.shape_material_mu, + self.k_start_body_contact, + ], + outputs=[ + self.body_body_contact_penalty_k, + self.body_body_contact_material_ke, + self.body_body_contact_material_kd, + self.body_body_contact_material_mu, + ], + dim=contact_launch_dim, + device=self.device, + ) # Warmstart AVBD penalty parameters for joints using the same cadence - # as contact history updates. + # as rigid history updates. if model.joint_count > 0: wp.launch( kernel=warmstart_joints, inputs=[ - model.joint_target_ke, + self.joint_penalty_k_max, self.joint_penalty_k_min, self.avbd_gamma, self.joint_penalty_k, # input/output ], - dim=model.joint_dof_count, + dim=self.joint_constraint_count, device=self.device, ) @@ -1078,11 +1300,10 @@ def initialize_rigid_bodies( model.joint_child, model.joint_X_p, model.joint_X_c, - model.joint_qd_start, - model.joint_target_ke, + self.joint_constraint_start, + self.joint_penalty_k_max, self.body_q_prev, # Use previous body transforms (start of step) for linearization model.body_q, # rest body transforms - model.body_com, self.joint_sigma_prev, self.joint_kappa_prev, self.joint_dkappa_prev, @@ -1100,7 +1321,7 @@ def initialize_rigid_bodies( # --------------------------- # Body-particle interaction # --------------------------- - if model.particle_count > 0 and update_rigid_history: + if model.particle_count > 0 and update_rigid_history and contacts is not None: # Build body-particle (rigid-particle) contact lists only when SolverVBD # is integrating rigid bodies itself; the external rigid solver path # does not use these per-body adjacency structures. Also skip if there @@ -1150,7 +1371,9 @@ def initialize_rigid_bodies( device=self.device, ) - def solve_particle_iteration(self, state_in: State, state_out: State, contacts: Contacts, dt: float, iter_num: int): + def solve_particle_iteration( + self, state_in: State, state_out: State, contacts: Contacts | None, dt: float, iter_num: int + ): """Solve one VBD iteration for particles.""" model = self.model @@ -1187,52 +1410,51 @@ def solve_particle_iteration(self, state_in: State, state_out: State, contacts: for color in range(len(model.particle_color_groups)): # Accumulate contact forces if self.particle_enable_self_contact: - if contacts is not None: - wp.launch( - kernel=accumulate_contact_force_and_hessian, - dim=self.collision_evaluation_kernel_launch_size, - inputs=[ - dt, - color, - self.particle_q_prev, - state_in.particle_q, - model.particle_colors, - model.tri_indices, - model.edge_indices, - # self-contact - self.trimesh_collision_info, - self.particle_self_contact_radius, - model.soft_contact_ke, - model.soft_contact_kd, - model.soft_contact_mu, - self.friction_epsilon, - self.trimesh_collision_detector.edge_edge_parallel_epsilon, - # body-particle contact - model.particle_radius, - contacts.soft_contact_particle, - contacts.soft_contact_count, - contacts.soft_contact_max, - self.body_particle_contact_penalty_k, - self.body_particle_contact_material_kd, - self.body_particle_contact_material_mu, - model.shape_material_mu, - model.shape_body, - body_q_for_particles, - body_q_prev_for_particles, - body_qd_for_particles, - model.body_com, - contacts.soft_contact_shape, - contacts.soft_contact_body_pos, - contacts.soft_contact_body_vel, - contacts.soft_contact_normal, - ], - outputs=[ - self.particle_forces, - self.particle_hessians, - ], - device=self.device, - max_blocks=model.device.sm_count, - ) + wp.launch( + kernel=accumulate_contact_force_and_hessian, + dim=self.collision_evaluation_kernel_launch_size, + inputs=[ + dt, + color, + self.particle_q_prev, + state_in.particle_q, + model.particle_colors, + model.tri_indices, + model.edge_indices, + # self-contact + self.trimesh_collision_info, + self.particle_self_contact_radius, + model.soft_contact_ke, + model.soft_contact_kd, + model.soft_contact_mu, + self.friction_epsilon, + self.trimesh_collision_detector.edge_edge_parallel_epsilon, + # body-particle contact + model.particle_radius, + contacts.soft_contact_particle, + contacts.soft_contact_count, + contacts.soft_contact_max, + self.body_particle_contact_penalty_k, + self.body_particle_contact_material_kd, + self.body_particle_contact_material_mu, + model.shape_material_mu, + model.shape_body, + body_q_for_particles, + body_q_prev_for_particles, + body_qd_for_particles, + model.body_com, + contacts.soft_contact_shape, + contacts.soft_contact_body_pos, + contacts.soft_contact_body_vel, + contacts.soft_contact_normal, + ], + outputs=[ + self.particle_forces, + self.particle_hessians, + ], + device=self.device, + max_blocks=model.device.sm_count, + ) else: wp.launch( kernel=accumulate_contact_force_and_hessian_no_self_contact, @@ -1256,7 +1478,7 @@ def solve_particle_iteration(self, state_in: State, state_out: State, contacts: model.shape_body, body_q_for_particles, body_q_prev_for_particles, - state_in.body_qd, + body_qd_for_particles, model.body_com, contacts.soft_contact_shape, contacts.soft_contact_body_pos, @@ -1423,7 +1645,7 @@ def solve_particle_iteration(self, state_in: State, state_out: State, contacts: device=self.device, ) - def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts: Contacts, dt: float): + def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts: Contacts | None, dt: float): """Solve one AVBD iteration for rigid bodies (per-iteration phase). Accumulates contact and joint forces/hessians, solves 6x6 rigid body systems per color, @@ -1439,7 +1661,7 @@ def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts # solve but still update body-particle soft-contact penalties so that adaptive # AVBD stiffness is used for cloth-rigid interaction. if self.integrate_with_external_rigid_solver: - if model.particle_count > 0: + if model.particle_count > 0 and contacts is not None: soft_contact_launch_dim = contacts.soft_contact_max wp.launch( kernel=update_duals_body_particle_contacts, @@ -1471,12 +1693,6 @@ def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts self.body_hessian_al.zero_() self.body_hessian_ll.zero_() - # AVBD stiffness arrays (adaptive penalties) - contact_stiffness_array = self.body_body_contact_penalty_k - joint_stiffness_array = self.joint_penalty_k - - # Use the Contacts buffer capacity as launch dimension - contact_launch_dim = contacts.rigid_contact_max body_color_groups = model.body_color_groups # Gauss-Seidel-style per-color updates @@ -1485,7 +1701,7 @@ def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts # Gauss-Seidel contact accumulation: evaluate contacts for bodies in this color # Accumulate body-particle forces and Hessians on bodies (per-body, per-color) - if model.particle_count > 0: + if model.particle_count > 0 and contacts is not None: wp.launch( kernel=accumulate_body_particle_contacts_per_body, dim=color_group.size * _NUM_CONTACT_THREADS_PER_BODY, @@ -1499,7 +1715,7 @@ def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts # rigid body state self.body_q_prev, state_in.body_q, - model.body_qd, + state_in.body_qd, model.body_com, model.body_inv_mass, # AVBD body-particle soft contact penalties and material properties @@ -1533,42 +1749,43 @@ def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts ) # Accumulate body-body (rigid-rigid) contact forces and Hessians on bodies (per-body, per-color) - wp.launch( - kernel=accumulate_body_body_contacts_per_body, - dim=color_group.size * _NUM_CONTACT_THREADS_PER_BODY, - inputs=[ - dt, - color_group, - self.body_q_prev, - state_in.body_q, - model.body_com, - model.body_inv_mass, - self.friction_epsilon, - contact_stiffness_array, - self.body_body_contact_material_kd, - self.body_body_contact_material_mu, - contacts.rigid_contact_count, - contacts.rigid_contact_shape0, - contacts.rigid_contact_shape1, - contacts.rigid_contact_point0, - contacts.rigid_contact_point1, - contacts.rigid_contact_normal, - contacts.rigid_contact_thickness0, - contacts.rigid_contact_thickness1, - model.shape_body, - self.body_body_contact_buffer_pre_alloc, - self.body_body_contact_counts, - self.body_body_contact_indices, - ], - outputs=[ - self.body_forces, - self.body_torques, - self.body_hessian_ll, - self.body_hessian_al, - self.body_hessian_aa, - ], - device=self.device, - ) + if contacts is not None: + wp.launch( + kernel=accumulate_body_body_contacts_per_body, + dim=color_group.size * _NUM_CONTACT_THREADS_PER_BODY, + inputs=[ + dt, + color_group, + self.body_q_prev, + state_in.body_q, + model.body_com, + model.body_inv_mass, + self.friction_epsilon, + self.body_body_contact_penalty_k, + self.body_body_contact_material_kd, + self.body_body_contact_material_mu, + contacts.rigid_contact_count, + contacts.rigid_contact_shape0, + contacts.rigid_contact_shape1, + contacts.rigid_contact_point0, + contacts.rigid_contact_point1, + contacts.rigid_contact_normal, + contacts.rigid_contact_thickness0, + contacts.rigid_contact_thickness1, + model.shape_body, + self.body_body_contact_buffer_pre_alloc, + self.body_body_contact_counts, + self.body_body_contact_indices, + ], + outputs=[ + self.body_forces, + self.body_torques, + self.body_hessian_ll, + self.body_hessian_al, + self.body_hessian_aa, + ], + device=self.device, + ) wp.launch( kernel=solve_rigid_body, @@ -1589,9 +1806,9 @@ def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts model.joint_child, model.joint_X_p, model.joint_X_c, - model.joint_qd_start, - model.joint_target_kd, - joint_stiffness_array, + self.joint_constraint_start, + self.joint_penalty_k, + self.joint_penalty_kd, self.joint_sigma_start, self.joint_C_fric, self.body_forces, @@ -1615,54 +1832,56 @@ def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts device=self.device, ) - # AVBD dual update: update adaptive penalties based on constraint violation - # Update body-body (rigid-rigid) contact penalties - wp.launch( - kernel=update_duals_body_body_contacts, - dim=contact_launch_dim, - inputs=[ - contacts.rigid_contact_count, - contacts.rigid_contact_shape0, - contacts.rigid_contact_shape1, - contacts.rigid_contact_point0, - contacts.rigid_contact_point1, - contacts.rigid_contact_normal, - contacts.rigid_contact_thickness0, - contacts.rigid_contact_thickness1, - model.shape_body, - state_out.body_q, - self.body_body_contact_material_ke, - self.avbd_beta, - self.body_body_contact_penalty_k, # input/output - ], - device=self.device, - ) - - # Update body-particle contact penalties - if model.particle_count > 0: - soft_contact_launch_dim = contacts.soft_contact_max + if contacts is not None: + # AVBD dual update: update adaptive penalties based on constraint violation + # Update body-body (rigid-rigid) contact penalties + contact_launch_dim = contacts.rigid_contact_max wp.launch( - kernel=update_duals_body_particle_contacts, - dim=soft_contact_launch_dim, + kernel=update_duals_body_body_contacts, + dim=contact_launch_dim, inputs=[ - contacts.soft_contact_count, - contacts.soft_contact_particle, - contacts.soft_contact_shape, - contacts.soft_contact_body_pos, - contacts.soft_contact_normal, - state_in.particle_q, - model.particle_radius, + contacts.rigid_contact_count, + contacts.rigid_contact_shape0, + contacts.rigid_contact_shape1, + contacts.rigid_contact_point0, + contacts.rigid_contact_point1, + contacts.rigid_contact_normal, + contacts.rigid_contact_thickness0, + contacts.rigid_contact_thickness1, model.shape_body, - # Rigid poses come from SolverVBD itself when - # integrate_with_external_rigid_solver=False - state_in.body_q, - self.body_particle_contact_material_ke, + state_out.body_q, + self.body_body_contact_material_ke, self.avbd_beta, - self.body_particle_contact_penalty_k, # input/output + self.body_body_contact_penalty_k, # input/output ], device=self.device, ) + # Update body-particle contact penalties + if model.particle_count > 0: + soft_contact_launch_dim = contacts.soft_contact_max + wp.launch( + kernel=update_duals_body_particle_contacts, + dim=soft_contact_launch_dim, + inputs=[ + contacts.soft_contact_count, + contacts.soft_contact_particle, + contacts.soft_contact_shape, + contacts.soft_contact_body_pos, + contacts.soft_contact_normal, + state_in.particle_q, + model.particle_radius, + model.shape_body, + # Rigid poses come from SolverVBD itself when + # integrate_with_external_rigid_solver=False + state_in.body_q, + self.body_particle_contact_material_ke, + self.avbd_beta, + self.body_particle_contact_penalty_k, # input/output + ], + device=self.device, + ) + # Update joint penalties at new positions wp.launch( kernel=update_duals_joint, @@ -1673,12 +1892,10 @@ def solve_rigid_body_iteration(self, state_in: State, state_out: State, contacts model.joint_child, model.joint_X_p, model.joint_X_c, - model.joint_qd_start, - model.joint_dof_dim, - model.joint_target_ke, + self.joint_constraint_start, + self.joint_penalty_k_max, state_out.body_q, model.body_q, - model.body_com, self.avbd_beta, self.joint_penalty_k, # input/output ], @@ -1728,11 +1945,10 @@ def finalize_rigid_bodies(self, state_out: State, dt: float): model.joint_child, model.joint_X_p, model.joint_X_c, - model.joint_qd_start, - model.joint_target_ke, + self.joint_constraint_start, + self.joint_penalty_k_max, state_out.body_q, model.body_q, - model.body_com, self.joint_dahl_eps_max, self.joint_dahl_tau, ], diff --git a/newton/examples/cable/example_cable_ball_joints.py b/newton/examples/cable/example_cable_ball_joints.py new file mode 100644 index 0000000000..5d3e821173 --- /dev/null +++ b/newton/examples/cable/example_cable_ball_joints.py @@ -0,0 +1,494 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################################### +# Example Cable Ball Joints +# +# Visual test for VBD BALL joints with kinematic anchors: +# - Create multiple kinematic anchor bodies (sphere, capsule, box, bear mesh) +# - Attach a cable (rod) to each anchor via a BALL joint +# - Drive anchors kinematically (translation or rotation) and verify cables follow +# +########################################################################### + +import math + +import numpy as np +import warp as wp +from pxr import Usd + +import newton +import newton.examples +import newton.usd + + +@wp.kernel +def compute_ball_anchor_error( + parent_ids: wp.array(dtype=wp.int32), + child_ids: wp.array(dtype=wp.int32), + parent_local: wp.array(dtype=wp.vec3), + child_local: wp.array(dtype=wp.vec3), + body_q: wp.array(dtype=wp.transform), + out_err: wp.array(dtype=float), +): + """Test-only: compute BALL joint anchor coincidence error. + + For each i, this computes the world-space distance between: + - parent anchor point: x_p = p_parent + R_parent * parent_local[i] + - child anchor point: x_c = p_child + R_child * child_local[i] + + Notes: + - This is a *verification helper* used by `Example.test_final()`, not part of the solver. + - We record only the *translation* part of the joint frames (local anchor positions). In this + example, `parent_xform` / `child_xform` are created with identity rotations, so this matches + the joint definition. + """ + i = wp.tid() + pb = parent_ids[i] + cb = child_ids[i] + + Xp = body_q[pb] + Xc = body_q[cb] + + pp = wp.transform_get_translation(Xp) + qp = wp.transform_get_rotation(Xp) + pc = wp.transform_get_translation(Xc) + qc = wp.transform_get_rotation(Xc) + + p_anchor = pp + wp.quat_rotate(qp, parent_local[i]) + c_anchor = pc + wp.quat_rotate(qc, child_local[i]) + + out_err[i] = wp.length(p_anchor - c_anchor) + + +@wp.kernel +def advance_time(t: wp.array(dtype=float), dt: float): + t[0] = t[0] + dt + + +@wp.kernel +def move_kinematic_anchors( + t: wp.array(dtype=float), + anchor_ids: wp.array(dtype=wp.int32), + base_pos: wp.array(dtype=wp.vec3), + phase: wp.array(dtype=float), + mode: wp.array(dtype=wp.int32), + ramp_time: float, + body_q: wp.array(dtype=wp.transform), + body_qd: wp.array(dtype=wp.spatial_vector), +): + """Kinematically animate anchor poses and keep their velocities at zero.""" + i = wp.tid() + b = anchor_ids[i] + + # Ramp in motion so initial velocity is small (independent of phase). + t0 = t[0] + u = wp.clamp(t0 / ramp_time, 0.0, 1.0) + ramp = u * u * (3.0 - 2.0 * u) # smoothstep + + ti = t0 + phase[i] + p0 = base_pos[i] + + w = 1.5 + m = mode[i] + + # mode == 0: translation-only (orthogonal to cable direction -Z) + # mode == 1: rotation-only (position fixed, rotate about +Y to move the attach point in XZ) + if m == 0: + amp_x = 0.35 + x = p0[0] + (ramp * amp_x) * wp.sin(w * ti) + pos = wp.vec3(x, p0[1], p0[2]) + q = wp.quat_identity() + else: + pos = p0 + # Larger rotation amplitude for the rotation-driven anchors + ang = (ramp * 1.6) * wp.sin(w * ti + 0.7) + q = wp.quat_from_axis_angle(wp.vec3(0.0, 1.0, 0.0), ang) + + body_q[b] = wp.transform(pos, q) + body_qd[b] = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +def _auto_scale(mesh, target_diameter: float) -> float: + verts = mesh.vertices + bb_min = verts.min(axis=0) + bb_max = verts.max(axis=0) + max_dim = float(np.max(bb_max - bb_min)) + return (target_diameter / max_dim) if max_dim > 1.0e-8 else 1.0 + + +def _make_straight_cable_down(anchor_world: wp.vec3, num_segments: int, segment_length: float): + """Rod centerline going downwards in world -Z, with per-segment quats for add_rod().""" + # Segment direction is -Z, so rotate local +Z -> world -Z (180deg around X) + q_seg = wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), math.pi) + + points = [anchor_world + wp.vec3(0.0, 0.0, -segment_length * i) for i in range(num_segments + 1)] + quats = [q_seg for _ in range(num_segments)] + return points, quats + + +class Example: + """Visual test for VBD BALL joints with kinematic anchors. + + High-level structure: + - Build several kinematic "driver" bodies (sphere/capsule/box/bear mesh). + - For each driver, create a straight cable (rod) hanging down in world -Z. + - Attach the first rod segment to the driver using a BALL joint (point-to-point attachment). + - Kinematically animate the drivers and verify the BALL joint keeps the two anchor points coincident. + """ + + def __init__(self, viewer, args=None): + self.viewer = viewer + self.args = args + + # Simulation cadence. + self.fps = 60 + self.frame_dt = 1.0 / self.fps + self.sim_substeps = 10 + self.sim_iterations = 1 + self.update_step_interval = 10 + self.sim_dt = self.frame_dt / self.sim_substeps + self.sim_time = 0.0 + + # Cable parameters. + num_segments = 50 + segment_length = 0.05 + cable_radius = 0.015 + bend_stiffness = 1.0e1 + bend_damping = 1.0e-2 + stretch_stiffness = 1.0e9 + stretch_damping = 0.0 + + # Default gravity (Z-up). Anchors are kinematic and moved explicitly. + builder = newton.ModelBuilder() + + # Contacts. + builder.default_shape_cfg.ke = 5.0e4 + builder.default_shape_cfg.kd = 5.0e1 + builder.default_shape_cfg.mu = 0.8 + + # Load meshes for variety. + bear_stage = Usd.Stage.Open(newton.examples.get_asset("bear.usd")) + bear_mesh = newton.usd.get_mesh(bear_stage.GetPrimAtPath("/root/bear/bear")) + + target_d = 0.35 + bear_scale = _auto_scale(bear_mesh, target_d) + + # For the bear, shift the mesh so the body origin is approximately at mid-height. + bear_verts = bear_mesh.vertices + bear_bb_min = bear_verts.min(axis=0) + bear_bb_max = bear_verts.max(axis=0) + bear_center_z = 0.5 * float(bear_bb_min[2] + bear_bb_max[2]) + bear_mesh_pz = -bear_center_z * bear_scale + + # Driver bodies (kinematic) + attached cables. + driver_specs = [ + ("sphere", None), + ("capsule", None), + ("box", None), + ("bear", (bear_mesh, bear_scale)), + ] + + # Anchors are kinematic and moved via a kernel. + self.anchor_bodies = [] + anchor_base_pos: list[wp.vec3] = [] + anchor_phase: list[float] = [] + anchor_mode: list[int] = [] + + # Test mode: record BALL joint anchor definitions so we can verify constraint satisfaction. + ball_parent_ids: list[int] = [] + ball_child_ids: list[int] = [] + ball_parent_local: list[wp.vec3] = [] + ball_child_local: list[wp.vec3] = [] + + # Spread the anchor+cable sets along Y (not X). + y0 = -1.2 + dy = 0.6 + + # Anchor height (raise this to give the cables more room to hang) + z0 = 4.0 + + # Two sets: translation-driven and rotation-driven (same shapes in both) + sets = [ + ("translate", -1.0, 0), # (label, x_offset, mode) + ("rotate", 1.0, 1), + ] + + for set_idx, (_label, x_offset, m) in enumerate(sets): + for i, (kind, mesh_info) in enumerate(driver_specs): + x = x_offset + y = y0 + dy * i + z = z0 + # Small per-shape vertical offsets (world -Z) for visual variety / clearance. + if kind == "sphere": + z = z0 - 0.05 + elif kind == "bear": + z = z0 - 0.10 + + # Anchor body: kinematic (will be driven by kernel). + body = builder.add_link(xform=wp.transform(wp.vec3(x, y, z), wp.quat_identity()), mass=0.0) + + # Add a visible collision shape + choose an anchor offset in local frame + # (initialize per-shape parameters for clarity/type-checkers) + r = 0.0 + hh = 0.0 + hx = 0.0 + hy = 0.0 + hz = 0.0 + attach_offset = 0.18 + key_prefix = f"{_label}_{kind}_{i}" + if kind == "sphere": + r = 0.18 + builder.add_shape_sphere(body=body, radius=r, key=f"drv_{key_prefix}") + attach_offset = r + elif kind == "capsule": + r = 0.12 + hh = 0.18 + # Lay the capsule down horizontally (capsule axis is +Z in shape-local frame). + capsule_q = wp.quat_from_axis_angle(wp.vec3(0.0, 1.0, 0.0), 0.5 * math.pi) + builder.add_shape_capsule( + body=body, + radius=r, + half_height=hh, + xform=wp.transform(p=wp.vec3(0.0, 0.0, 0.0), q=capsule_q), + key=f"drv_{key_prefix}", + ) + attach_offset = r + elif kind == "box": + hx, hy, hz = 0.18, 0.12, 0.10 + builder.add_shape_box(body=body, hx=hx, hy=hy, hz=hz, key=f"drv_{key_prefix}") + attach_offset = hz + else: + mesh, scale = mesh_info + builder.add_shape_mesh( + body=body, + mesh=mesh, + scale=(scale, scale, scale), + xform=wp.transform(p=wp.vec3(0.0, 0.0, bear_mesh_pz), q=wp.quat_identity()), + key=f"drv_{key_prefix}", + ) + + # Make the driver strictly kinematic (override any mass/inertia contributed by shapes). + builder.body_mass[body] = 0.0 + builder.body_inv_mass[body] = 0.0 + builder.body_inertia[body] = wp.mat33(0.0) + builder.body_inv_inertia[body] = wp.mat33(0.0) + + # Cable root anchor in world at the "bottom" of the driver. + # Keep a small +X offset so the rotation-driven anchors produce visible attachment-point + # motion when rotating about +Y. + cable_attach_x = 0.1 + if kind in ("capsule", "box"): + cable_attach_x = 0.2 + dz_body = z0 - z + parent_anchor_local = wp.vec3(cable_attach_x, 0.0, -attach_offset + dz_body) + anchor_world = wp.vec3(x + cable_attach_x, y, z0 - attach_offset) + + if kind in ("sphere", "capsule", "box"): + x_local = cable_attach_x + if kind == "sphere": + r = attach_offset + # Ensure the anchor point lies on the sphere surface: x^2 + z^2 = r^2 (y=0). + z_local = -math.sqrt(max(r * r - x_local * x_local, 0.0)) + elif kind == "capsule": + # Capsule is laid horizontally with its axis along body-local +X. + # Surface in XZ (y=0): cylinder for |x|<=hh (z=-r), hemispheres beyond. + r = attach_offset + hh_f = hh + x_clamped = max(min(x_local, hh_f + r), -(hh_f + r)) + dx = abs(x_clamped) - hh_f + if dx <= 0.0: + z_local = -r + else: + z_local = -math.sqrt(max(r * r - dx * dx, 0.0)) + x_local = x_clamped + else: + # Box: clamp the requested x offset to the box face so we stay on the surface. + hx_f = hx + hz_f = hz + x_local = max(min(x_local, hx_f), -hx_f) + z_local = -hz_f + + parent_anchor_local = wp.vec3(x_local, 0.0, z_local) + # Use the actual body pose (x,y,z), not the uniform z0, so the cable touches the shape. + anchor_world = wp.vec3(x + x_local, y, z + z_local) + rod_points, rod_quats = _make_straight_cable_down(anchor_world, num_segments, segment_length) + + rod_bodies, rod_joints = builder.add_rod( + positions=rod_points, + quaternions=rod_quats, + radius=cable_radius, + bend_stiffness=bend_stiffness, + bend_damping=bend_damping, + stretch_stiffness=stretch_stiffness, + stretch_damping=stretch_damping, + key=f"cable_{key_prefix}", + # Build one articulation including both the ball joint and all rod joints. + wrap_in_articulation=False, + ) + + # Attach cable start point to driver with a ball joint. + # `add_rod()` convention: + # - rod body i has its *body origin* at `positions[i]` (segment start) + # - the capsule shape (and COM) are offset by +half_height along the body's local +Z + # + # Therefore, the cable "start endpoint" is located at body-local z=0 for `rod_bodies[0]`. + child_anchor_local = wp.vec3(0.0, 0.0, 0.0) + j_ball = builder.add_joint_ball( + parent=body, + child=rod_bodies[0], + parent_xform=wp.transform(parent_anchor_local, wp.quat_identity()), + child_xform=wp.transform(child_anchor_local, wp.quat_identity()), + key=f"attach_{key_prefix}", + ) + # Put all joints (rod cable joints + the ball attachment) into one articulation. + # Builder requires joint indices be monotonically increasing and contiguous. + # We create rod joints first, then the ball joint, so the correct order is: + builder.add_articulation([*rod_joints, j_ball]) + + # Record anchor point definitions for testing (world-space error should be near-zero). + ball_parent_ids.append(int(body)) + ball_child_ids.append(int(rod_bodies[0])) + ball_parent_local.append(parent_anchor_local) + ball_child_local.append(child_anchor_local) + + self.anchor_bodies.append(body) + anchor_base_pos.append(wp.vec3(x, y, z)) + # Different velocities via phase + set offset + anchor_phase.append(0.6 * i + 1.3 * set_idx) + anchor_mode.append(int(m)) + + builder.add_ground_plane() + builder.color() + self.model = builder.finalize() + + # Stiffen ball constraint caps (non-cable joints) so the attachment behaves near-hard. + self.solver = newton.solvers.SolverVBD( + self.model, + iterations=self.sim_iterations, + friction_epsilon=0.1, + rigid_joint_linear_ke=1.0e9, + rigid_joint_linear_k_start=1.0e5, + ) + + self.state_0 = self.model.state() + self.state_1 = self.model.state() + self.control = self.model.control() + self.contacts = self.model.collide(self.state_0) + + self.viewer.set_model(self.model) + + # Device-side kinematic anchor motion buffers (graph-capture friendly) + self.device = self.solver.device + self.anchor_bodies_wp = wp.array(self.anchor_bodies, dtype=wp.int32, device=self.device) + self.anchor_base_pos_wp = wp.array(anchor_base_pos, dtype=wp.vec3, device=self.device) + self.anchor_phase_wp = wp.array(anchor_phase, dtype=float, device=self.device) + self.anchor_mode_wp = wp.array(anchor_mode, dtype=wp.int32, device=self.device) + self.sim_time_array = wp.zeros(1, dtype=float, device=self.device) + + # Test buffers (ball joint constraint satisfaction) + self._ball_parent_ids = wp.array(ball_parent_ids, dtype=wp.int32, device=self.device) + self._ball_child_ids = wp.array(ball_child_ids, dtype=wp.int32, device=self.device) + self._ball_parent_local = wp.array(ball_parent_local, dtype=wp.vec3, device=self.device) + self._ball_child_local = wp.array(ball_child_local, dtype=wp.vec3, device=self.device) + + self.capture() + + def capture(self): + if self.solver.device.is_cuda: + with wp.ScopedCapture() as capture: + self.simulate() + self.graph = capture.graph + else: + self.graph = None + + def simulate(self): + for substep in range(self.sim_substeps): + self.state_0.clear_forces() + self.viewer.apply_forces(self.state_0) + + # Kinematic anchor swing (orthogonal to cable direction -Z) + wp.launch( + kernel=move_kinematic_anchors, + dim=self.anchor_bodies_wp.shape[0], + inputs=[ + self.sim_time_array, + self.anchor_bodies_wp, + self.anchor_base_pos_wp, + self.anchor_phase_wp, + self.anchor_mode_wp, + 1.0, # ramp_time [s] + self.state_0.body_q, + self.state_0.body_qd, + ], + device=self.device, + ) + + update_step_history = (substep % self.update_step_interval) == 0 + + if update_step_history: + self.contacts = self.model.collide(self.state_0) + + self.solver.set_rigid_history_update(update_step_history) + self.solver.step(self.state_0, self.state_1, self.control, self.contacts, self.sim_dt) + self.state_0, self.state_1 = self.state_1, self.state_0 + + # Advance time for anchor motion (on device) + wp.launch(kernel=advance_time, dim=1, inputs=[self.sim_time_array, self.sim_dt], device=self.device) + + def step(self): + if self.graph: + wp.capture_launch(self.graph) + else: + self.simulate() + + self.sim_time += self.frame_dt + + def render(self): + self.viewer.begin_frame(self.sim_time) + self.viewer.log_state(self.state_0) + self.viewer.log_contacts(self.contacts, self.state_0) + self.viewer.end_frame() + + def test_final(self): + # Verify ball joint anchor points are coincident (within tolerance) at the final state. + # Loose tolerance: these are stiff but not perfectly rigid constraints. + if self._ball_parent_ids.shape[0] == 0: + return + + ball_err = wp.zeros(self._ball_parent_ids.shape[0], dtype=float, device=self.device) + wp.launch( + kernel=compute_ball_anchor_error, + dim=ball_err.shape[0], + inputs=[ + self._ball_parent_ids, + self._ball_child_ids, + self._ball_parent_local, + self._ball_child_local, + self.state_0.body_q, + ball_err, + ], + device=self.device, + ) + err_max = float(np.max(ball_err.numpy())) + tol = 1.0e-3 + if err_max > tol: + raise AssertionError(f"BALL joint anchor error too large: max={err_max:.6g} > tol={tol}") + + +if __name__ == "__main__": + viewer, args = newton.examples.init() + example = Example(viewer, args) + newton.examples.run(example, args) diff --git a/newton/examples/cable/example_cable_bend.py b/newton/examples/cable/example_cable_bend.py index f713007bc0..9821013af0 100644 --- a/newton/examples/cable/example_cable_bend.py +++ b/newton/examples/cable/example_cable_bend.py @@ -52,8 +52,8 @@ def create_cable_geometry(self, pos: wp.vec3 | None = None, num_elements=10, len Returns: Tuple of (points, edge_indices, quaternions): - - points: List of capsule center positions (num_elements + 1). - - edge_indices: Flattened array of edge connectivity (2*num_elements). + - points: List of segment endpoints in world space (num_elements + 1). + - edge_indices: Flattened array of edge connectivity (2*num_elements). (Not used by `add_rod()`.) - quaternions: List of capsule orientations using parallel transport (num_elements). """ if num_elements <= 0: @@ -158,6 +158,7 @@ def __init__(self, viewer, args=None): builder.default_shape_cfg.mu = 1.0 # Friction coefficient y_separation = 0.5 + self.cable_bodies_list: list[list[int]] = [] # Create 5 cables in a row along the y-axis, centered around origin for i, bend_stiffness in enumerate(bend_stiffness_values): @@ -190,6 +191,11 @@ def __init__(self, viewer, args=None): first_body = rod_bodies[0] builder.body_mass[first_body] = 0.0 builder.body_inv_mass[first_body] = 0.0 + builder.body_inertia[first_body] = wp.mat33(0.0) + builder.body_inv_inertia[first_body] = wp.mat33(0.0) + + # Store full body index list for each cable for robust testing. + self.cable_bodies_list.append(rod_bodies) # Add ground plane builder.add_ground_plane() @@ -275,11 +281,10 @@ def test_final(self): assert (np.abs(body_velocities) < 5e2).all(), "Body velocities too large (>500)" # Test 2: Check cable connectivity (joint constraints) - for cable_idx in range(self.num_cables): - start_body = cable_idx * self.num_elements - for segment in range(self.num_elements - 1): - body1_idx = start_body + segment - body2_idx = start_body + segment + 1 + for cable_idx, rod_bodies in enumerate(self.cable_bodies_list): + for segment in range(len(rod_bodies) - 1): + body1_idx = rod_bodies[segment] + body2_idx = rod_bodies[segment + 1] pos1 = body_positions[body1_idx][:3] # Extract translation part pos2 = body_positions[body2_idx][:3] @@ -307,9 +312,10 @@ def test_final(self): assert min_z >= -ground_tolerance, f"Cable fell below ground: min_z = {min_z:.3f} < {-ground_tolerance:.3f}" # Test 5: Basic physics check - cables should hang down due to gravity - # Compare first and last segment positions of first cable - first_segment_z = body_positions[0, 2] - last_segment_z = body_positions[self.num_elements - 1, 2] + # Compare the anchored end and free end of the first cable. + first_cable = self.cable_bodies_list[0] + first_segment_z = body_positions[first_cable[0], 2] + last_segment_z = body_positions[first_cable[-1], 2] assert last_segment_z < first_segment_z, ( f"Cable not hanging properly: last segment z={last_segment_z:.3f} should be < first segment z={first_segment_z:.3f}" ) diff --git a/newton/examples/cable/example_cable_bend_damping.py b/newton/examples/cable/example_cable_bend_damping.py index 1f19e044aa..a9104d44ab 100644 --- a/newton/examples/cable/example_cable_bend_damping.py +++ b/newton/examples/cable/example_cable_bend_damping.py @@ -49,8 +49,8 @@ def create_cable_geometry(self, pos: wp.vec3 | None = None, num_elements=10, len Returns: Tuple of (points, edge_indices, quaternions): - - points: List of capsule center positions (num_elements + 1). - - edge_indices: Flattened array of edge connectivity (2*num_elements). + - points: List of segment endpoints in world space (num_elements + 1). + - edge_indices: Flattened array of edge connectivity (2*num_elements). (Not used by `add_rod()`.) - quaternions: List of capsule orientations using parallel transport (num_elements). """ if pos is None: @@ -164,7 +164,7 @@ def __init__(self, viewer, args=None): y_separation = 0.5 - # Create 5 cables in a row along the y-axis, centered around origin + # Create cables in a row along the y-axis, centered around origin for i, bend_damping in enumerate(bend_damping_values): # Center cables around origin: vary by y_separation y_pos = (i - (self.num_cables - 1) / 2.0) * y_separation @@ -195,6 +195,8 @@ def __init__(self, viewer, args=None): first_body = rod_bodies[0] builder.body_mass[first_body] = 0.0 builder.body_inv_mass[first_body] = 0.0 + builder.body_inertia[first_body] = wp.mat33(0.0) + builder.body_inv_inertia[first_body] = wp.mat33(0.0) kinematic_body_indices.append(first_body) # Store full body index list for each cable for robust testing @@ -227,7 +229,7 @@ def __init__(self, viewer, args=None): self.capture() def capture(self): - if wp.get_device().is_cuda: + if self.solver.device.is_cuda: with wp.ScopedCapture() as capture: self.simulate() self.graph = capture.graph @@ -323,10 +325,11 @@ def test_final(self): ) assert min_z >= -ground_tolerance, f"Cable fell below ground: min_z = {min_z:.3f} < {-ground_tolerance:.3f}" - # Test 5: Basic physics check - cables should hang down due to gravity - # Compare first and last segment positions of first cable - first_segment_z = body_positions[0, 2] - last_segment_z = body_positions[self.num_elements - 1, 2] + # Test 5: Basic physics check - cables should sag under gravity. + # Compare the anchored end and free end of the first cable. + first_cable = self.cable_bodies_list[0] + first_segment_z = body_positions[first_cable[0], 2] + last_segment_z = body_positions[first_cable[-1], 2] assert last_segment_z < first_segment_z, ( f"Cable not hanging properly: last segment z={last_segment_z:.3f} should be < first segment z={first_segment_z:.3f}" ) diff --git a/newton/examples/cable/example_cable_bundle_hysteresis.py b/newton/examples/cable/example_cable_bundle_hysteresis.py index bc3d99bcd8..53158088a6 100644 --- a/newton/examples/cable/example_cable_bundle_hysteresis.py +++ b/newton/examples/cable/example_cable_bundle_hysteresis.py @@ -193,6 +193,10 @@ def __init__( stretch_damping = 0.0 builder = newton.ModelBuilder() + builder.rigid_contact_margin = 0.05 + + # Register solver-specific custom attributes (Dahl plasticity parameters live on the Model) + newton.solvers.SolverVBD.register_custom_attributes(builder) builder.gravity = -9.81 # Set default material properties for cables (cable-to-cable contact) @@ -274,9 +278,11 @@ def __init__( body=body, radius=obstacle_radius, half_height=obstacle_half_height, cfg=obstacle_cfg ) - # Make obstacle kinematic (zero mass) + # Make obstacle kinematic builder.body_mass[body] = 0.0 builder.body_inv_mass[body] = 0.0 + builder.body_inertia[body] = wp.mat33(0.0) + builder.body_inv_inertia[body] = wp.mat33(0.0) self.obstacle_bodies.append(body) obstacle_init_z_list.append(float(z)) @@ -299,14 +305,18 @@ def __init__( # Finalize model self.model = builder.finalize() + # Author Dahl friction parameters (per-joint) via custom model attributes. + # These are read by SolverVBD when rigid_enable_dahl_friction=True. + if hasattr(self.model, "vbd"): + self.model.vbd.dahl_eps_max.fill_(float(eps_max)) + self.model.vbd.dahl_tau.fill_(float(tau)) + # Create VBD solver with Dahl friction (cable bending hysteresis) self.solver = newton.solvers.SolverVBD( self.model, iterations=self.sim_iterations, friction_epsilon=0.1, rigid_enable_dahl_friction=with_dahl, - rigid_dahl_eps_max=eps_max, - rigid_dahl_tau=tau, ) # Initialize states and contacts diff --git a/newton/examples/cable/example_cable_fixed_joints.py b/newton/examples/cable/example_cable_fixed_joints.py new file mode 100644 index 0000000000..ecce12c597 --- /dev/null +++ b/newton/examples/cable/example_cable_fixed_joints.py @@ -0,0 +1,528 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +########################################################################### +# Example Cable Fixed Joints +# +# Visual test for VBD FIXED joints with kinematic anchors: +# - Create multiple kinematic anchor bodies (sphere, capsule, box, bear mesh) +# - Attach a cable (rod) to each anchor via a FIXED joint +# - Drive anchors kinematically (translation or rotation) and verify cables follow +# +########################################################################### + +import math + +import numpy as np +import warp as wp +from pxr import Usd + +import newton +import newton.examples +import newton.usd + + +@wp.kernel +def compute_fixed_joint_error( + parent_ids: wp.array(dtype=wp.int32), + child_ids: wp.array(dtype=wp.int32), + parent_local: wp.array(dtype=wp.vec3), + child_local: wp.array(dtype=wp.vec3), + parent_frame_q: wp.array(dtype=wp.quat), + child_frame_q: wp.array(dtype=wp.quat), + body_q: wp.array(dtype=wp.transform), + out_err_pos: wp.array(dtype=float), + out_err_ang: wp.array(dtype=float), +): + """Test-only: compute FIXED joint error (position + angle) using the joint frames. + + For each i: + - Position error: ||x_p - x_c|| + - Angular error: angle(q_pframe^{-1} * q_cframe) [rad] + + Notes: + - This is a *verification helper* used by `Example.test_final()`, not part of the solver. + - We compare the world-space joint frame rotations: + q_pframe = q_parent * parent_frame_q[i] + q_cframe = q_child * child_frame_q[i] + """ + i = wp.tid() + pb = parent_ids[i] + cb = child_ids[i] + + Xp = body_q[pb] + Xc = body_q[cb] + + pp = wp.transform_get_translation(Xp) + qp = wp.transform_get_rotation(Xp) + pc = wp.transform_get_translation(Xc) + qc = wp.transform_get_rotation(Xc) + + p_anchor = pp + wp.quat_rotate(qp, parent_local[i]) + c_anchor = pc + wp.quat_rotate(qc, child_local[i]) + out_err_pos[i] = wp.length(p_anchor - c_anchor) + + # Angular difference between joint frames: dq = q_pframe^{-1} * q_cframe + qpf = wp.mul(qp, parent_frame_q[i]) + qcf = wp.mul(qc, child_frame_q[i]) + dq = wp.mul(wp.quat_inverse(qpf), qcf) + dq = wp.normalize(dq) + # q and -q represent the same rotation; use abs(w) to get the shortest-angle measure. + w = wp.clamp(wp.abs(dq[3]), 0.0, 1.0) + out_err_ang[i] = 2.0 * wp.acos(w) + + +@wp.kernel +def advance_time(t: wp.array(dtype=float), dt: float): + t[0] = t[0] + dt + + +@wp.kernel +def move_kinematic_anchors( + t: wp.array(dtype=float), + anchor_ids: wp.array(dtype=wp.int32), + base_pos: wp.array(dtype=wp.vec3), + phase: wp.array(dtype=float), + mode: wp.array(dtype=wp.int32), + ramp_time: float, + body_q: wp.array(dtype=wp.transform), + body_qd: wp.array(dtype=wp.spatial_vector), +): + """Kinematically animate anchor poses and keep their velocities at zero.""" + i = wp.tid() + b = anchor_ids[i] + + # Ramp in motion so initial velocity is small (independent of phase). + t0 = t[0] + u = wp.clamp(t0 / ramp_time, 0.0, 1.0) + ramp = u * u * (3.0 - 2.0 * u) # smoothstep + + ti = t0 + phase[i] + p0 = base_pos[i] + + w = 1.5 + m = mode[i] + + # mode == 0: translation-only (orthogonal to cable direction -Z) + # mode == 1: rotation-only (position fixed, rotate about +Y to move the attach point in XZ) + if m == 0: + amp_x = 0.35 + x = p0[0] + (ramp * amp_x) * wp.sin(w * ti) + pos = wp.vec3(x, p0[1], p0[2]) + q = wp.quat_identity() + else: + pos = p0 + # Larger rotation amplitude for the rotation-driven anchors + ang = (ramp * 1.6) * wp.sin(w * ti + 0.7) + q = wp.quat_from_axis_angle(wp.vec3(0.0, 1.0, 0.0), ang) + + body_q[b] = wp.transform(pos, q) + body_qd[b] = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +def _auto_scale(mesh, target_diameter: float) -> float: + verts = mesh.vertices + bb_min = verts.min(axis=0) + bb_max = verts.max(axis=0) + max_dim = float(np.max(bb_max - bb_min)) + return (target_diameter / max_dim) if max_dim > 1.0e-8 else 1.0 + + +def _make_straight_cable_down(anchor_world: wp.vec3, num_segments: int, segment_length: float): + """Rod centerline going downwards in world -Z, with per-segment quats for add_rod().""" + # Segment direction is -Z, so rotate local +Z -> world -Z (180deg around X) + q_seg = wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), math.pi) + + points = [anchor_world + wp.vec3(0.0, 0.0, -segment_length * i) for i in range(num_segments + 1)] + quats = [q_seg for _ in range(num_segments)] + return points, quats + + +class Example: + """Visual test for VBD FIXED joints with kinematic anchors. + + High-level structure: + - Build several kinematic "driver" bodies (sphere/capsule/box/bear mesh). + - For each driver, create a straight cable (rod) hanging down in world -Z. + - Attach the first rod segment to the driver using a FIXED joint (welded pose at the attachment). + - Kinematically animate the drivers and verify the FIXED joint keeps the joint frames coincident. + """ + + def __init__(self, viewer, args=None): + self.viewer = viewer + self.args = args + + # Simulation cadence. + self.fps = 60 + self.frame_dt = 1.0 / self.fps + self.sim_substeps = 10 + self.sim_iterations = 1 + self.update_step_interval = 10 + self.sim_dt = self.frame_dt / self.sim_substeps + self.sim_time = 0.0 + + # Cable parameters. + num_segments = 50 + segment_length = 0.05 + cable_radius = 0.015 + bend_stiffness = 1.0e1 + bend_damping = 1.0e-2 + stretch_stiffness = 1.0e9 + stretch_damping = 0.0 + + # Default gravity (Z-up). Anchors are kinematic and moved explicitly. + builder = newton.ModelBuilder() + + # Contacts. + builder.default_shape_cfg.ke = 5.0e4 + builder.default_shape_cfg.kd = 5.0e1 + builder.default_shape_cfg.mu = 0.8 + + # Load meshes for variety. + bear_stage = Usd.Stage.Open(newton.examples.get_asset("bear.usd")) + bear_mesh = newton.usd.get_mesh(bear_stage.GetPrimAtPath("/root/bear/bear")) + + target_d = 0.35 + bear_scale = _auto_scale(bear_mesh, target_d) + + # For the bear, shift the mesh so the body origin is approximately at mid-height. + bear_verts = bear_mesh.vertices + bear_bb_min = bear_verts.min(axis=0) + bear_bb_max = bear_verts.max(axis=0) + bear_center_z = 0.5 * float(bear_bb_min[2] + bear_bb_max[2]) + bear_mesh_pz = -bear_center_z * bear_scale + + # Driver bodies (kinematic) + attached cables. + driver_specs = [ + ("sphere", None), + ("capsule", None), + ("box", None), + ("bear", (bear_mesh, bear_scale)), + ] + + # Anchors are kinematic and moved via a kernel. + self.anchor_bodies = [] + anchor_base_pos: list[wp.vec3] = [] + anchor_phase: list[float] = [] + anchor_mode: list[int] = [] + + # Test mode: record FIXED joint anchor definitions so we can verify constraint satisfaction. + fixed_parent_ids: list[int] = [] + fixed_child_ids: list[int] = [] + fixed_parent_local: list[wp.vec3] = [] + fixed_child_local: list[wp.vec3] = [] + fixed_parent_frame_q: list[wp.quat] = [] + fixed_child_frame_q: list[wp.quat] = [] + + # Spread the anchor+cable sets along Y (not X). + y0 = -1.2 + dy = 0.6 + + # Anchor height (raise this to give the cables more room to hang) + z0 = 4.0 + + # Two sets: translation-driven and rotation-driven (same shapes in both) + sets = [ + ("translate", -1.0, 0), # (label, x_offset, mode) + ("rotate", 1.0, 1), + ] + + for set_idx, (_label, x_offset, m) in enumerate(sets): + for i, (kind, mesh_info) in enumerate(driver_specs): + x = x_offset + y = y0 + dy * i + z = z0 + # Small per-shape vertical offsets (world -Z) for visual variety / clearance. + if kind == "sphere": + z = z0 - 0.05 + elif kind == "bear": + z = z0 - 0.10 + + # Anchor body: kinematic (will be driven by kernel). + body = builder.add_link(xform=wp.transform(wp.vec3(x, y, z), wp.quat_identity()), mass=0.0) + + # Add a visible collision shape + choose an anchor offset in local frame + # (initialize per-shape parameters for clarity/type-checkers) + r = 0.0 + hh = 0.0 + hx = 0.0 + hy = 0.0 + hz = 0.0 + attach_offset = 0.18 + key_prefix = f"{_label}_{kind}_{i}" + if kind == "sphere": + r = 0.18 + builder.add_shape_sphere(body=body, radius=r, key=f"drv_{key_prefix}") + attach_offset = r + elif kind == "capsule": + r = 0.12 + hh = 0.18 + # Lay the capsule down horizontally (capsule axis is +Z in shape-local frame). + capsule_q = wp.quat_from_axis_angle(wp.vec3(0.0, 1.0, 0.0), 0.5 * math.pi) + builder.add_shape_capsule( + body=body, + radius=r, + half_height=hh, + xform=wp.transform(p=wp.vec3(0.0, 0.0, 0.0), q=capsule_q), + key=f"drv_{key_prefix}", + ) + attach_offset = r + elif kind == "box": + hx, hy, hz = 0.18, 0.12, 0.10 + builder.add_shape_box(body=body, hx=hx, hy=hy, hz=hz, key=f"drv_{key_prefix}") + attach_offset = hz + else: + mesh, scale = mesh_info + builder.add_shape_mesh( + body=body, + mesh=mesh, + scale=(scale, scale, scale), + xform=wp.transform(p=wp.vec3(0.0, 0.0, bear_mesh_pz), q=wp.quat_identity()), + key=f"drv_{key_prefix}", + ) + + # Make the driver strictly kinematic (override any mass/inertia contributed by shapes). + builder.body_mass[body] = 0.0 + builder.body_inv_mass[body] = 0.0 + builder.body_inertia[body] = wp.mat33(0.0) + builder.body_inv_inertia[body] = wp.mat33(0.0) + + # Cable root anchor in world at the "bottom" of the driver. + # We keep a small +X offset so the rotation-driven anchors produce visible motion of the + # attachment point when rotating about +Y. + cable_attach_x = 0.1 + if kind in ("capsule", "box"): + cable_attach_x = 0.2 + dz_body = z0 - z + parent_anchor_local = wp.vec3(cable_attach_x, 0.0, -attach_offset + dz_body) + anchor_world = wp.vec3(x + cable_attach_x, y, z0 - attach_offset) + + if kind in ("sphere", "capsule", "box"): + x_local = cable_attach_x + if kind == "sphere": + r = attach_offset + # Ensure the anchor point lies on the sphere surface: x^2 + z^2 = r^2 (y=0). + z_local = -math.sqrt(max(r * r - x_local * x_local, 0.0)) + elif kind == "capsule": + # Capsule is laid horizontally with its axis along body-local +X. + # Surface in XZ (y=0): cylinder for |x|<=hh (z=-r), hemispheres beyond. + r = attach_offset + hh_f = hh + x_clamped = max(min(x_local, hh_f + r), -(hh_f + r)) + dx = abs(x_clamped) - hh_f + if dx <= 0.0: + z_local = -r + else: + z_local = -math.sqrt(max(r * r - dx * dx, 0.0)) + x_local = x_clamped + else: + # Box: clamp the requested x offset to the box face so we stay on the surface. + hx_f = hx + hz_f = hz + x_local = max(min(x_local, hx_f), -hx_f) + z_local = -hz_f + + parent_anchor_local = wp.vec3(x_local, 0.0, z_local) + # Use the actual body pose (x,y,z), not the uniform z0, so the cable touches the shape. + anchor_world = wp.vec3(x + x_local, y, z + z_local) + + rod_points, rod_quats = _make_straight_cable_down(anchor_world, num_segments, segment_length) + + rod_bodies, rod_joints = builder.add_rod( + positions=rod_points, + quaternions=rod_quats, + radius=cable_radius, + bend_stiffness=bend_stiffness, + bend_damping=bend_damping, + stretch_stiffness=stretch_stiffness, + stretch_damping=stretch_damping, + key=f"cable_{key_prefix}", + # Build one articulation including both the fixed joint and all rod joints. + wrap_in_articulation=False, + ) + + # Attach cable start point to driver with a fixed joint. + # `add_rod()` convention: + # - rod body i has its *body origin* at `positions[i]` (segment start) + # - the capsule shape (and COM) are offset by +half_height along the body's local +Z + # + # Therefore, the cable "start endpoint" is located at body-local z=0 for `rod_bodies[0]`. + child_anchor_local = wp.vec3(0.0, 0.0, 0.0) + # Define the FIXED joint "rest" using joint frames so the rod's natural segment + # orientation (pointing down) matches at t=0. + parent_frame_q = rod_quats[0] + child_frame_q = wp.quat_identity() + j_fixed = builder.add_joint_fixed( + parent=body, + child=rod_bodies[0], + parent_xform=wp.transform(parent_anchor_local, parent_frame_q), + child_xform=wp.transform(child_anchor_local, child_frame_q), + key=f"attach_{key_prefix}", + ) + # Put all joints (rod cable joints + the fixed attachment) into one articulation. + # Builder requires joint indices be monotonically increasing and contiguous. + # We create rod joints first, then the fixed joint, so the correct order is: + builder.add_articulation([*rod_joints, j_fixed]) + + # Record anchor point definitions for testing (world-space errors should be near-zero). + fixed_parent_ids.append(int(body)) + fixed_child_ids.append(int(rod_bodies[0])) + fixed_parent_local.append(parent_anchor_local) + fixed_child_local.append(child_anchor_local) + fixed_parent_frame_q.append(parent_frame_q) + fixed_child_frame_q.append(child_frame_q) + + self.anchor_bodies.append(body) + anchor_base_pos.append(wp.vec3(x, y, z)) + # Different velocities via phase + set offset + anchor_phase.append(0.6 * i + 1.3 * set_idx) + anchor_mode.append(int(m)) + + builder.add_ground_plane() + builder.color() + self.model = builder.finalize() + + # Stiffen fixed constraint caps (non-cable joints) so the attachment behaves near-hard. + self.solver = newton.solvers.SolverVBD( + self.model, + iterations=self.sim_iterations, + friction_epsilon=0.1, + rigid_joint_linear_ke=1.0e9, + rigid_joint_angular_ke=1.0e9, + rigid_joint_linear_k_start=1.0e5, + ) + + self.state_0 = self.model.state() + self.state_1 = self.model.state() + self.control = self.model.control() + self.contacts = self.model.collide(self.state_0) + + self.viewer.set_model(self.model) + + # Device-side kinematic anchor motion buffers (graph-capture friendly) + self.device = self.solver.device + self.anchor_bodies_wp = wp.array(self.anchor_bodies, dtype=wp.int32, device=self.device) + self.anchor_base_pos_wp = wp.array(anchor_base_pos, dtype=wp.vec3, device=self.device) + self.anchor_phase_wp = wp.array(anchor_phase, dtype=float, device=self.device) + self.anchor_mode_wp = wp.array(anchor_mode, dtype=wp.int32, device=self.device) + self.sim_time_array = wp.zeros(1, dtype=float, device=self.device) + + # Test buffers (fixed joint constraint satisfaction) + self._fixed_parent_ids = wp.array(fixed_parent_ids, dtype=wp.int32, device=self.device) + self._fixed_child_ids = wp.array(fixed_child_ids, dtype=wp.int32, device=self.device) + self._fixed_parent_local = wp.array(fixed_parent_local, dtype=wp.vec3, device=self.device) + self._fixed_child_local = wp.array(fixed_child_local, dtype=wp.vec3, device=self.device) + self._fixed_parent_frame_q = wp.array(fixed_parent_frame_q, dtype=wp.quat, device=self.device) + self._fixed_child_frame_q = wp.array(fixed_child_frame_q, dtype=wp.quat, device=self.device) + + self.capture() + + def capture(self): + if self.solver.device.is_cuda: + with wp.ScopedCapture() as capture: + self.simulate() + self.graph = capture.graph + else: + self.graph = None + + def simulate(self): + for substep in range(self.sim_substeps): + self.state_0.clear_forces() + self.viewer.apply_forces(self.state_0) + + # Kinematic anchor swing (orthogonal to cable direction -Z) + wp.launch( + kernel=move_kinematic_anchors, + dim=self.anchor_bodies_wp.shape[0], + inputs=[ + self.sim_time_array, + self.anchor_bodies_wp, + self.anchor_base_pos_wp, + self.anchor_phase_wp, + self.anchor_mode_wp, + 1.0, # ramp_time [s] + self.state_0.body_q, + self.state_0.body_qd, + ], + device=self.device, + ) + + update_step_history = (substep % self.update_step_interval) == 0 + + if update_step_history: + self.contacts = self.model.collide(self.state_0) + + self.solver.set_rigid_history_update(update_step_history) + self.solver.step(self.state_0, self.state_1, self.control, self.contacts, self.sim_dt) + self.state_0, self.state_1 = self.state_1, self.state_0 + + # Advance time for anchor motion (on device) + wp.launch(kernel=advance_time, dim=1, inputs=[self.sim_time_array, self.sim_dt], device=self.device) + + def step(self): + if self.graph: + wp.capture_launch(self.graph) + else: + self.simulate() + + self.sim_time += self.frame_dt + + def render(self): + self.viewer.begin_frame(self.sim_time) + self.viewer.log_state(self.state_0) + self.viewer.log_contacts(self.contacts, self.state_0) + self.viewer.end_frame() + + def test_final(self): + # Verify fixed joint frames are coincident (within tolerance) at the final state. + # Loose tolerances: these are stiff but not perfectly rigid constraints. + if self._fixed_parent_ids.shape[0] == 0: + return + + err_pos = wp.zeros(self._fixed_parent_ids.shape[0], dtype=float, device=self.device) + err_ang = wp.zeros(self._fixed_parent_ids.shape[0], dtype=float, device=self.device) + wp.launch( + kernel=compute_fixed_joint_error, + dim=err_pos.shape[0], + inputs=[ + self._fixed_parent_ids, + self._fixed_child_ids, + self._fixed_parent_local, + self._fixed_child_local, + self._fixed_parent_frame_q, + self._fixed_child_frame_q, + self.state_0.body_q, + err_pos, + err_ang, + ], + device=self.device, + ) + err_pos_max = float(np.max(err_pos.numpy())) + err_ang_max = float(np.max(err_ang.numpy())) + + tol_pos = 1.0e-3 + tol_ang = 1.0e-2 + if err_pos_max > tol_pos or err_ang_max > tol_ang: + raise AssertionError( + "FIXED joint error too large: " + f"pos_max={err_pos_max:.6g} (tol={tol_pos}), " + f"ang_max={err_ang_max:.6g} (tol={tol_ang})" + ) + + +if __name__ == "__main__": + viewer, args = newton.examples.init() + example = Example(viewer, args) + newton.examples.run(example, args) diff --git a/newton/examples/cable/example_cable_twist.py b/newton/examples/cable/example_cable_twist.py index 9615822dfa..faeafef5d0 100644 --- a/newton/examples/cable/example_cable_twist.py +++ b/newton/examples/cable/example_cable_twist.py @@ -36,13 +36,14 @@ def spin_first_capsules_kernel( body_indices: wp.array(dtype=wp.int32), twist_rates: wp.array(dtype=float), # radians per second per body dt: float, - body_q: wp.array(dtype=wp.transform), + body_q0: wp.array(dtype=wp.transform), + body_q1: wp.array(dtype=wp.transform), ): """Apply continuous twist to the first segment of each cable.""" tid = wp.tid() body_id = body_indices[tid] - t = body_q[body_id] + t = body_q0[body_id] pos = wp.transform_get_translation(t) rot = wp.transform_get_rotation(t) @@ -52,7 +53,9 @@ def spin_first_capsules_kernel( dq = wp.quat_from_axis_angle(axis_world, angle) rot_new = wp.mul(dq, rot) - body_q[body_id] = wp.transform(pos, rot_new) + T = wp.transform(pos, rot_new) + body_q0[body_id] = T + body_q1[body_id] = T class Example: @@ -73,8 +76,8 @@ def create_cable_geometry_with_turns( Returns: Tuple of (points, edge_indices, quaternions): - - points: List of capsule center positions (num_elements + 1). - - edge_indices: Flattened array of edge connectivity (2*num_elements). + - points: List of segment endpoints in world space (num_elements + 1). + - edge_indices: Flattened array of edge connectivity (2*num_elements). (Not used by `add_rod()`.) - quaternions: List of capsule orientations using parallel transport (num_elements). """ if pos is None: @@ -232,6 +235,8 @@ def __init__(self, viewer, args=None): first_body = rod_bodies[0] builder.body_mass[first_body] = 0.0 builder.body_inv_mass[first_body] = 0.0 + builder.body_inertia[first_body] = wp.mat33(0.0) + builder.body_inv_inertia[first_body] = wp.mat33(0.0) kinematic_body_indices.append(first_body) # Store for twist application and testing @@ -270,7 +275,7 @@ def __init__(self, viewer, args=None): def capture(self): """Capture simulation loop into a CUDA graph for optimal GPU performance.""" - if wp.get_device().is_cuda: + if self.solver.device.is_cuda: with wp.ScopedCapture() as capture: self.simulate() self.graph = capture.graph @@ -287,7 +292,7 @@ def simulate(self): kernel=spin_first_capsules_kernel, dim=self.kinematic_bodies.shape[0], inputs=[self.kinematic_bodies, self.first_twist_rates, self.sim_dt], - outputs=[self.state_0.body_q], + outputs=[self.state_0.body_q, self.state_1.body_q], ) # Apply forces to the model diff --git a/newton/tests/test_cable.py b/newton/tests/test_cable.py index 6dd54b5cea..dab1669b68 100644 --- a/newton/tests/test_cable.py +++ b/newton/tests/test_cable.py @@ -24,6 +24,140 @@ devices = get_test_devices() +# ----------------------------------------------------------------------------- +# Assert helpers +# ----------------------------------------------------------------------------- + + +def _assert_bodies_above_ground( + test: unittest.TestCase, + body_q: np.ndarray, + body_ids: list[int], + context: str, + margin: float = 1.0e-4, +) -> None: + """Assert a set of bodies are not below the z=0 ground plane (within margin).""" + z_pos = body_q[body_ids, 2] + z_min = z_pos.min() + test.assertGreaterEqual( + z_min, + -margin, + msg=f"{context}: body below ground: z_min={z_min:.6f} < {-margin:.6f}", + ) + + +def _assert_capsule_attachments( + test: unittest.TestCase, + body_q: np.ndarray, + body_ids: list[int], + context: str, + segment_length: float, + tol_ratio: float = 0.05, +) -> None: + """Assert that adjacent capsules remain attached within tolerance. + + Approximates the parent capsule end and child capsule start in world space and + checks that their separation is small relative to the rest capsule length. + """ + tol = tol_ratio * segment_length + for i in range(len(body_ids) - 1): + idx_p = body_ids[i] + idx_c = body_ids[i + 1] + + p_pos = body_q[idx_p, :3] + c_pos = body_q[idx_c, :3] + + dir_vec = c_pos - p_pos + seg_len = np.linalg.norm(dir_vec) + if seg_len > 1.0e-6: + dir_hat = dir_vec / seg_len + else: + dir_hat = np.array([1.0, 0.0, 0.0], dtype=float) + + parent_end = p_pos + dir_hat * segment_length + child_start = c_pos + gap = np.linalg.norm(parent_end - child_start) + + test.assertLessEqual( + gap, + tol, + msg=f"{context}: capsule attachment gap too large at segment {i} (gap={gap:.6g}, tol={tol:.6g})", + ) + + +def _assert_surface_attachment( + test: unittest.TestCase, + body_q: np.ndarray, + anchor_body: int, + child_body: int, + context: str, + parent_anchor_local: wp.vec3, + tol: float = 1.0e-3, +) -> None: + """Assert that the child body origin lies on the anchor-frame attachment point. + + Intended attach point (world): + x_expected = x_anchor + R_anchor * parent_anchor_local + """ + with wp.ScopedDevice("cpu"): + x_anchor = wp.vec3(body_q[anchor_body][0], body_q[anchor_body][1], body_q[anchor_body][2]) + q_anchor = wp.quat( + body_q[anchor_body][3], body_q[anchor_body][4], body_q[anchor_body][5], body_q[anchor_body][6] + ) + x_expected = x_anchor + wp.quat_rotate(q_anchor, parent_anchor_local) + + x_child = wp.vec3(body_q[child_body][0], body_q[child_body][1], body_q[child_body][2]) + err = float(wp.length(x_child - x_expected)) + test.assertLess( + err, + tol, + msg=f"{context}: surface-attachment error is {err:.6e} (tol={tol:.1e})", + ) + + +# ----------------------------------------------------------------------------- +# Warp kernels +# ----------------------------------------------------------------------------- + + +@wp.kernel +def _set_kinematic_body_pose( + body_id: wp.int32, + pose: wp.transform, + body_q: wp.array(dtype=wp.transform), + body_qd: wp.array(dtype=wp.spatial_vector), +): + body_q[body_id] = pose + body_qd[body_id] = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +# ----------------------------------------------------------------------------- +# Geometry helpers +# ----------------------------------------------------------------------------- + + +def _make_straight_cable_along_x(num_elements: int, segment_length: float, z_height: float): + """Create points/quats for `ModelBuilder.add_rod()` with a straight cable along +X. + + Notes: + - Points are centered about x=0 (first point is at x=-0.5*cable_length). + - Capsules have local +Z as their axis; quaternions rotate local +Z to world +X. + """ + cable_length = num_elements * segment_length + start_x = -0.5 * cable_length + points = [wp.vec3(start_x + i * segment_length, 0.0, z_height) for i in range(num_elements + 1)] + + # Capsule internal axis is +Z; rotate so rod axis aligns with +X + rot_z_to_x = wp.quat_between_vectors(wp.vec3(0.0, 0.0, 1.0), wp.vec3(1.0, 0.0, 0.0)) + edge_q = [rot_z_to_x] * num_elements + return points, edge_q + + +# ----------------------------------------------------------------------------- +# Model builders +# ----------------------------------------------------------------------------- + + def _build_cable_chain( device, num_links: int = 6, @@ -50,21 +184,7 @@ def _build_cable_chain( # Geometry: straight cable along +X, centered around the origin num_elements = num_links - cable_length = num_elements * segment_length - - # Points: segment endpoints along X at some height - z_height = 3.0 - start_x = -0.5 * cable_length - points = [] - for i in range(num_elements + 1): - x = start_x + i * segment_length - points.append(wp.vec3(x, 0.0, z_height)) - - # Capsule internal axis is +Z; rotate so rod axis aligns with +X - # Use a single orientation for all segments since the cable is straight. - # quat_between_vectors(local +Z, world +X) gives the minimal rotation that aligns the axes. - rot_z_to_x = wp.quat_between_vectors(wp.vec3(0.0, 0.0, 1.0), wp.vec3(1.0, 0.0, 0.0)) - edge_q = [rot_z_to_x] * num_elements + points, edge_q = _make_straight_cable_along_x(num_elements, segment_length, z_height=3.0) # Create a rod-based cable rod_bodies, _rod_joints = builder.add_rod( @@ -113,7 +233,7 @@ def _build_cable_loop(device, num_links: int = 6): points = [] for i in range(num_elements + 1): # For a closed loop we wrap the last point back to the first - angle = 2.0 * wp.pi * float(i) / float(num_elements) + angle = 2.0 * wp.pi * (i / num_elements) x = radius * wp.cos(angle) y = radius * wp.sin(angle) points.append(wp.vec3(x, y, z_height)) @@ -151,42 +271,86 @@ def _build_cable_loop(device, num_links: int = 6): return model, state0, state1, control -def _assert_capsule_attachments( - test: unittest.TestCase, - body_q: np.ndarray, - rod_bodies: list[int], - segment_length: float, - context: str, -) -> None: - """Assert that adjacent capsules remain attached within 5% of segment length. +# ----------------------------------------------------------------------------- +# Compute helpers +# ----------------------------------------------------------------------------- - Approximates the parent capsule end and child capsule start in world space and - checks that their separation is small relative to the rest capsule length. + +def _compute_ball_joint_anchor_error(model: newton.Model, body_q: wp.array, joint_id: int) -> float: + """Compute BALL joint world-space anchor error (CPU float). + + Returns: + |x_c - x_p|, where x_p and x_c are the parent/child anchor positions in world space. """ - tol = 0.05 * segment_length - for i in range(len(rod_bodies) - 1): - idx_p = rod_bodies[i] - idx_c = rod_bodies[i + 1] - - p = body_q[idx_p, :3] - c = body_q[idx_c, :3] - - dir_vec = c - p - seg_len_now = np.linalg.norm(dir_vec) - if seg_len_now > 1.0e-6: - dir = dir_vec / seg_len_now - else: - dir = np.array([1.0, 0.0, 0.0], dtype=float) + with wp.ScopedDevice("cpu"): + jp = model.joint_parent.numpy()[joint_id].item() + jc = model.joint_child.numpy()[joint_id].item() + X_p = model.joint_X_p.numpy()[joint_id] + X_c = model.joint_X_c.numpy()[joint_id] - parent_end = p + dir * segment_length - child_start = c - dist = np.linalg.norm(parent_end - child_start) + # wp.transform is [p(3), q(4)] in xyzw order + X_pj = wp.transform(wp.vec3(X_p[0], X_p[1], X_p[2]), wp.quat(X_p[3], X_p[4], X_p[5], X_p[6])) + X_cj = wp.transform(wp.vec3(X_c[0], X_c[1], X_c[2]), wp.quat(X_c[3], X_c[4], X_c[5], X_c[6])) - test.assertLessEqual( - dist, - tol, - msg=f"{context}: capsule attachment gap too large at segment {i} (dist={dist}, tol={tol})", - ) + bq = body_q.to("cpu").numpy() + q_p = bq[jp] + q_c = bq[jc] + T_p = wp.transform(wp.vec3(q_p[0], q_p[1], q_p[2]), wp.quat(q_p[3], q_p[4], q_p[5], q_p[6])) + T_c = wp.transform(wp.vec3(q_c[0], q_c[1], q_c[2]), wp.quat(q_c[3], q_c[4], q_c[5], q_c[6])) + + X_wp = T_p * X_pj + X_wc = T_c * X_cj + x_p = wp.transform_get_translation(X_wp) + x_c = wp.transform_get_translation(X_wc) + return float(wp.length(x_c - x_p)) + + +def _compute_fixed_joint_frame_error(model: newton.Model, body_q: wp.array, joint_id: int) -> tuple[float, float]: + """Compute FIXED joint world-space frame error (CPU floats). + + Returns: + (pos_err, ang_err) + + - pos_err: |x_c - x_p| where x_p/x_c are the parent/child joint-frame translations in world space. + - ang_err: relative rotation angle between joint-frame orientations in world space [rad]. + """ + with wp.ScopedDevice("cpu"): + jp = model.joint_parent.numpy()[joint_id].item() + jc = model.joint_child.numpy()[joint_id].item() + X_p = model.joint_X_p.numpy()[joint_id] + X_c = model.joint_X_c.numpy()[joint_id] + + # wp.transform is [p(3), q(4)] in xyzw order + X_pj = wp.transform(wp.vec3(X_p[0], X_p[1], X_p[2]), wp.quat(X_p[3], X_p[4], X_p[5], X_p[6])) + X_cj = wp.transform(wp.vec3(X_c[0], X_c[1], X_c[2]), wp.quat(X_c[3], X_c[4], X_c[5], X_c[6])) + + bq = body_q.to("cpu").numpy() + q_p = bq[jp] + q_c = bq[jc] + T_p = wp.transform(wp.vec3(q_p[0], q_p[1], q_p[2]), wp.quat(q_p[3], q_p[4], q_p[5], q_p[6])) + T_c = wp.transform(wp.vec3(q_c[0], q_c[1], q_c[2]), wp.quat(q_c[3], q_c[4], q_c[5], q_c[6])) + + X_wp = T_p * X_pj + X_wc = T_c * X_cj + + x_p = wp.transform_get_translation(X_wp) + x_c = wp.transform_get_translation(X_wc) + pos_err = float(wp.length(x_c - x_p)) + + q_wp = wp.transform_get_rotation(X_wp) + q_wc = wp.transform_get_rotation(X_wc) + q_rel = wp.mul(wp.quat_inverse(q_wp), q_wc) + q_rel = wp.normalize(q_rel) + # Quaternion sign is arbitrary; enforce shortest-path angle for robustness. + w = wp.clamp(wp.abs(q_rel[3]), 0.0, 1.0) + ang_err = float(2.0 * wp.acos(w)) + + return pos_err, ang_err + + +# ----------------------------------------------------------------------------- +# Test implementations +# ----------------------------------------------------------------------------- def _cable_chain_connectivity_impl(test: unittest.TestCase, device): @@ -268,43 +432,78 @@ def _cable_loop_connectivity_impl(test: unittest.TestCase, device): def _cable_bend_stiffness_impl(test: unittest.TestCase, device): """Cable VBD: bend stiffness sweep should have a noticeable effect on tip position.""" - # From soft to stiff + # From soft to stiff. Build multiple cables in one model. bend_values = [1.0e1, 1.0e2, 1.0e3] - tip_heights = [] segment_length = 0.2 + num_links = 10 + + builder = newton.ModelBuilder() + builder.default_shape_cfg.ke = 1.0e2 + builder.default_shape_cfg.kd = 1.0e1 + builder.default_shape_cfg.mu = 1.0 - for k in bend_values: - model, state0, state1, control, rod_bodies = _build_cable_chain( - device, num_links=10, bend_stiffness=k, bend_damping=1.0e1, segment_length=segment_length + # Place cables far apart along Y so they don't interact. + y_offsets = [-5.0, 0.0, 5.0] + tip_bodies: list[int] = [] + all_rod_bodies: list[list[int]] = [] + + for k, y0 in zip(bend_values, y_offsets, strict=True): + points, edge_q = _make_straight_cable_along_x(num_links, segment_length, z_height=3.0) + points = [wp.vec3(p[0], p[1] + y0, p[2]) for p in points] + + rod_bodies, _rod_joints = builder.add_rod( + positions=points, + quaternions=edge_q, + radius=0.05, + bend_stiffness=k, + bend_damping=1.0e1, + stretch_stiffness=1.0e6, + stretch_damping=1.0e-2, + key=f"bend_stiffness_{k:.0e}", ) - solver = newton.solvers.SolverVBD(model, iterations=10) - frame_dt = 1.0 / 60.0 - sim_substeps = 10 - sim_dt = frame_dt / sim_substeps - - # Run for a short duration to let bending respond to gravity - for _ in range(20): - for _ in range(sim_substeps): - state0.clear_forces() - contacts = model.collide(state0) - solver.step(state0, state1, control, contacts, sim_dt) - state0, state1 = state1, state0 - - final_q = state0.body_q.numpy() - tip_body = rod_bodies[-1] - tip_heights.append(float(final_q[tip_body, 2])) - - # Check capsule attachments for this dynamic configuration + + # Pin the first body of each cable. + first_body = rod_bodies[0] + builder.body_mass[first_body] = 0.0 + builder.body_inv_mass[first_body] = 0.0 + + all_rod_bodies.append(rod_bodies) + tip_bodies.append(rod_bodies[-1]) + + builder.color() + model = builder.finalize(device=device) + model.set_gravity((0.0, 0.0, -9.81)) + + state0, state1 = model.state(), model.state() + control = model.control() + solver = newton.solvers.SolverVBD(model, iterations=10) + + frame_dt = 1.0 / 60.0 + sim_substeps = 10 + sim_dt = frame_dt / sim_substeps + num_steps = 20 + + # Run for a short duration to let bending respond to gravity + for _step in range(num_steps): + for _substep in range(sim_substeps): + state0.clear_forces() + contacts = model.collide(state0) + solver.step(state0, state1, control, contacts, sim_dt) + state0, state1 = state1, state0 + + final_q = state0.body_q.numpy() + tip_heights = np.array([final_q[tip_body, 2] for tip_body in tip_bodies], dtype=float) + + # Check capsule attachments for each dynamic configuration + for k, rod_bodies in zip(bend_values, all_rod_bodies, strict=True): _assert_capsule_attachments( test, body_q=final_q, - rod_bodies=rod_bodies, + body_ids=rod_bodies, segment_length=segment_length, context=f"Bend stiffness {k}", ) - tip_heights = np.array(tip_heights, dtype=float) - # Check that stiffer cables have higher tip positions (less sagging under gravity) # Expect monotonic increase: tip_heights[0] < tip_heights[1] < tip_heights[2] for i in range(len(tip_heights) - 1): @@ -313,8 +512,8 @@ def _cable_bend_stiffness_impl(test: unittest.TestCase, device): tip_heights[i + 1], msg=( f"Stiffer cable should have higher tip (less sag): " - f"k={bend_values[i]:.1e} → z={tip_heights[i]:.4f}, " - f"k={bend_values[i + 1]:.1e} → z={tip_heights[i + 1]:.4f}" + f"k={bend_values[i]:.1e} -> z={tip_heights[i]:.4f}, " + f"k={bend_values[i + 1]:.1e} -> z={tip_heights[i + 1]:.4f}" ), ) @@ -334,13 +533,14 @@ def _cable_sagging_and_stability_impl(test: unittest.TestCase, device): frame_dt = 1.0 / 60.0 sim_substeps = 10 sim_dt = frame_dt / sim_substeps + num_steps = 20 # Record initial positions initial_q = state0.body_q.numpy().copy() z_initial = initial_q[:, 2] - for _ in range(20): - for _ in range(sim_substeps): + for _step in range(num_steps): + for _substep in range(sim_substeps): state0.clear_forces() contacts = model.collide(state0) solver.step(state0, state1, control, contacts, sim_dt) @@ -353,9 +553,9 @@ def _cable_sagging_and_stability_impl(test: unittest.TestCase, device): test.assertTrue((z_final < z_initial).any()) # Positions should remain within a band relative to initial height and cable length - z0 = float(z_initial.min()) + z0 = z_initial.min() x_initial = initial_q[:, 0] - cable_length = float(x_initial.max() - x_initial.min()) + cable_length = x_initial.max() - x_initial.min() lower_bound = z0 - 2.0 * cable_length upper_bound = z0 + 2.0 * cable_length @@ -442,10 +642,11 @@ def _cable_twist_response_impl(test: unittest.TestCase, device): frame_dt = 1.0 / 60.0 sim_substeps = 10 sim_dt = frame_dt / sim_substeps + num_steps = 20 # Run a short simulation to let twist propagate - for _ in range(20): - for _ in range(sim_substeps): + for _step in range(num_steps): + for _substep in range(sim_substeps): state0.clear_forces() contacts = model.collide(state0) solver.step(state0, state1, control, contacts, sim_dt) @@ -455,14 +656,14 @@ def _cable_twist_response_impl(test: unittest.TestCase, device): # Check capsule attachments remain good _assert_capsule_attachments( - test, body_q=final_q, rod_bodies=rod_bodies, segment_length=segment_length, context="Twist" + test, body_q=final_q, body_ids=rod_bodies, segment_length=segment_length, context="Twist" ) # Check that the child orientation has changed significantly due to twist q_child_final = final_q[child_body, 3:] # Quaternion dot product magnitude indicates orientation similarity (1 => identical up to sign) - dot = float(abs(np.dot(q_child_initial, q_child_final))) + dot = abs(np.dot(q_child_initial, q_child_final)) test.assertLess( dot, 0.999, @@ -477,7 +678,7 @@ def _cable_twist_response_impl(test: unittest.TestCase, device): def get_tip_pos(body_idx, q_all): p = q_all[body_idx, :3] q = q_all[body_idx, 3:] # x, y, z, w - rot = wp.quat(float(q[0]), float(q[1]), float(q[2]), float(q[3])) + rot = wp.quat(q[0], q[1], q[2], q[3]) v = wp.vec3(0.0, 0.0, segment_length) v_rot = wp.quat_rotate(rot, v) return np.array([p[0] + v_rot[0], p[1] + v_rot[1], p[2] + v_rot[2]]) @@ -488,17 +689,19 @@ def get_tip_pos(body_idx, q_all): tol = 0.1 * segment_length # X and Z should stay close to their original values + tip_x0, tip_y0, tip_z0 = tip_initial + tip_x1, tip_y1, tip_z1 = tip_final test.assertAlmostEqual( - float(tip_final[0]), - float(tip_initial[0]), + tip_x1, + tip_x0, delta=tol, - msg=f"Twist: expected child tip x to stay near {float(tip_initial[0])}, got {float(tip_final[0])}", + msg=f"Twist: expected child tip x to stay near {tip_x0}, got {tip_x1}", ) test.assertAlmostEqual( - float(tip_final[2]), - float(tip_initial[2]), + tip_z1, + tip_z0, delta=tol, - msg=f"Twist: expected child tip z to stay near {float(tip_initial[2])}, got {float(tip_final[2])}", + msg=f"Twist: expected child tip z to stay near {tip_z0}, got {tip_z1}", ) # Y should approximately flip sign (reflect across the X-Z plane) @@ -506,18 +709,14 @@ def get_tip_pos(body_idx, q_all): # We check if the sign is flipped, but allow for some deviation in magnitude # because the twist might not be perfectly 180 degrees or there might be some energy loss/damping test.assertTrue( - float(tip_final[1]) * float(tip_initial[1]) < 0, - msg=f"Twist: expected child tip y to flip sign from {float(tip_initial[1])}, got {float(tip_final[1])}", + tip_y1 * tip_y0 < 0, + msg=f"Twist: expected child tip y to flip sign from {tip_y0}, got {tip_y1}", ) test.assertAlmostEqual( - float(tip_final[1]), - float(-tip_initial[1]), + tip_y1, + -tip_y0, delta=tol, - msg=( - "Twist: expected child tip y magnitude to be similar " - f"from {abs(float(tip_initial[1]))}, " - f"got {abs(float(tip_final[1]))}" - ), + msg=(f"Twist: expected child tip y magnitude to be similar from {abs(tip_y0)}, got {abs(tip_y1)}"), ) @@ -567,12 +766,13 @@ def _two_layer_cable_pile_collision_impl(test: unittest.TestCase, device): # Build two layers: bottom layer along X, top layer along Y # Both layers centered at origin (0, 0) in horizontal plane + cable_bodies: list[int] = [] for layer in range(2): orient = "x" if layer == 0 else "y" z0 = base_height + layer * layer_gap for lane in range(2): - # Symmetric offset: lane 0 → -0.5*spacing, lane 1 → +0.5*spacing + # Symmetric offset: lane 0 -> -0.5*spacing, lane 1 -> +0.5*spacing # This centers both layers at the same (x,y) = (0,0) position offset = (lane - 0.5) * lane_spacing @@ -600,7 +800,7 @@ def _two_layer_cable_pile_collision_impl(test: unittest.TestCase, device): rot = wp.quat_between_vectors(local_axis, cable_direction) edge_q = [rot] * num_elements - builder.add_rod( + rod_bodies, _rod_joints = builder.add_rod( positions=points, quaternions=edge_q, radius=cable_radius, @@ -610,6 +810,7 @@ def _two_layer_cable_pile_collision_impl(test: unittest.TestCase, device): stretch_damping=1.0e-2, key=f"pile_l{layer}_{lane}", ) + cable_bodies.extend(rod_bodies) builder.color() model = builder.finalize(device=device) @@ -626,8 +827,8 @@ def _two_layer_cable_pile_collision_impl(test: unittest.TestCase, device): # Let the pile settle under gravity and contact num_steps = 20 - for _ in range(num_steps): - for _ in range(sim_substeps): + for _step in range(num_steps): + for _substep in range(sim_substeps): state0.clear_forces() contacts = model.collide(state0) solver.step(state0, state1, control, contacts, sim_dt) @@ -639,15 +840,22 @@ def _two_layer_cable_pile_collision_impl(test: unittest.TestCase, device): # Basic sanity checks test.assertTrue(np.isfinite(positions).all(), "Non-finite positions detected in cable pile") + _assert_bodies_above_ground( + test, + body_q=body_q, + body_ids=cable_bodies, + margin=0.25 * cable_width, + context="Cable pile", + ) # Define vertical bands with a small margin for soft contact tolerance # Increased margin to account for contact compression and stiff cable deformation margin = 0.1 * cable_width - # Bottom layer should live between ground and one cable width (±margin) + # Bottom layer should live between ground and one cable width (+/- margin) bottom_band = (z_positions >= -margin) & (z_positions <= cable_width + margin) - # Second layer between one and two cable widths (±margin) + # Second layer between one and two cable widths (+/- margin) top_band = (z_positions >= cable_width - margin) & (z_positions <= 2.0 * cable_width + margin) # All bodies should fall within one of the two bands @@ -656,15 +864,15 @@ def _two_layer_cable_pile_collision_impl(test: unittest.TestCase, device): np.all(in_bands), msg=( "Some cable bodies lie outside the expected two-layer vertical bands: " - f"min_z={float(z_positions.min()):.4f}, max_z={float(z_positions.max()):.4f}, " + f"min_z={z_positions.min():.4f}, max_z={z_positions.max():.4f}, " f"cable_width={cable_width:.4f}, expected in [0, {2.0 * cable_width + margin:.4f}] " f"with band margin {margin:.4f}." ), ) # Ensure we actually formed two distinct layers - num_bottom = np.sum(bottom_band) - num_top = np.sum(top_band) + num_bottom = int(np.sum(bottom_band)) + num_top = int(np.sum(top_band)) test.assertGreater( num_bottom, @@ -681,16 +889,272 @@ def _two_layer_cable_pile_collision_impl(test: unittest.TestCase, device): total_bodies = len(z_positions) test.assertGreater( num_bottom, - total_bodies * 0.1, + 0.1 * total_bodies, msg=f"Bottom layer has too few bodies: {num_bottom}/{total_bodies} (< 10%)", ) test.assertGreater( num_top, - total_bodies * 0.1, + 0.1 * total_bodies, msg=f"Top layer has too few bodies: {num_top}/{total_bodies} (< 10%)", ) +def _cable_ball_joint_attaches_rod_endpoint_impl(test: unittest.TestCase, device): + """Cable VBD: BALL joint should keep rod start endpoint attached to a kinematic anchor.""" + builder = newton.ModelBuilder() + builder.default_shape_cfg.ke = 1.0e2 + builder.default_shape_cfg.kd = 1.0e1 + builder.default_shape_cfg.mu = 1.0 + + # Kinematic anchor body at the rod start point. + anchor_pos = wp.vec3(0.0, 0.0, 3.0) + anchor = builder.add_body(xform=wp.transform(anchor_pos, wp.quat_identity())) + builder.body_mass[anchor] = 0.0 + builder.body_inv_mass[anchor] = 0.0 + # Anchor marker sphere. + anchor_radius = 0.1 + builder.add_shape_sphere(anchor, radius=anchor_radius) + + # Build a straight cable (rod) and attach its start endpoint to the anchor with a BALL joint. + num_elements = 20 + segment_length = 0.05 + points, edge_q = _make_straight_cable_along_x(num_elements, segment_length, z_height=anchor_pos[2]) + rod_radius = 0.01 + # Attach the cable endpoint to the sphere surface (not the center), accounting for cable radius so the + # capsule endcap surface and the sphere surface are coincident along the rod axis (+X). + attach_offset = wp.float32(anchor_radius + rod_radius) + parent_anchor_local = wp.vec3(attach_offset, 0.0, 0.0) # parent local == world (identity rotation) + anchor_world_attach = anchor_pos + wp.vec3(attach_offset, 0.0, 0.0) + + # Reposition the generated cable so its first point coincides with the sphere-surface attach point. + # (The helper builds a cable centered about x=0.) + p0 = points[0] + offset = anchor_world_attach - p0 + points = [p + offset for p in points] + + rod_bodies, rod_joints = builder.add_rod( + positions=points, + quaternions=edge_q, + radius=rod_radius, + bend_stiffness=1.0e-1, + bend_damping=1.0e-2, + stretch_stiffness=1.0e9, + stretch_damping=0.0, + wrap_in_articulation=False, + key="test_cable_ball_joint_attach", + ) + + # `add_rod()` convention: rod body origin is at `positions[i]` (segment start), so the start endpoint is at z=0 local. + child_anchor_local = wp.vec3(0.0, 0.0, 0.0) + j_ball = builder.add_joint_ball( + parent=anchor, + child=rod_bodies[0], + parent_xform=wp.transform(parent_anchor_local, wp.quat_identity()), + child_xform=wp.transform(child_anchor_local, wp.quat_identity()), + ) + builder.add_articulation([*rod_joints, j_ball]) + + builder.add_ground_plane() + builder.color() + model = builder.finalize(device=device) + model.set_gravity((0.0, 0.0, -9.81)) + + state0 = model.state() + state1 = model.state() + control = model.control() + + solver = newton.solvers.SolverVBD( + model, + iterations=10, + ) + + # Smoothly move the anchor with substeps (mirrors cable example pattern). + frame_dt = 1.0 / 60.0 + sim_substeps = 10 + sim_dt = frame_dt / sim_substeps + num_steps = 20 + + for _step in range(num_steps): + for _substep in range(sim_substeps): + t = (_step * sim_substeps + _substep) * sim_dt + dx = wp.float32(0.05 * np.sin(1.5 * t)) + + pose = wp.transform(wp.vec3(dx, 0.0, anchor_pos[2]), wp.quat_identity()) + wp.launch( + _set_kinematic_body_pose, + dim=1, + inputs=[wp.int32(anchor), pose, state0.body_q, state0.body_qd], + device=device, + ) + + contacts = model.collide(state0) + solver.step(state0, state1, control, contacts, dt=sim_dt) + state0, state1 = state1, state0 + + err = _compute_ball_joint_anchor_error(model, state0.body_q, j_ball) + test.assertLess(err, 1.0e-3) + + # Also verify the rod joints remained well-attached along the chain. + final_q = state0.body_q.numpy() + test.assertTrue(np.isfinite(final_q).all(), "Non-finite body transforms detected in BALL joint test") + _assert_surface_attachment( + test, + body_q=final_q, + anchor_body=anchor, + child_body=rod_bodies[0], + context="Cable BALL joint attachment", + parent_anchor_local=parent_anchor_local, + ) + + _assert_bodies_above_ground( + test, + body_q=final_q, + body_ids=rod_bodies, + margin=0.25 * segment_length, + context="Cable BALL joint attachment", + ) + _assert_capsule_attachments( + test, + body_q=final_q, + body_ids=rod_bodies, + segment_length=segment_length, + context="Cable BALL joint attachment", + ) + + +def _cable_fixed_joint_attaches_rod_endpoint_impl(test: unittest.TestCase, device): + """Cable VBD: FIXED joint should keep rod start frame welded to a kinematic anchor.""" + builder = newton.ModelBuilder() + builder.default_shape_cfg.ke = 1.0e2 + builder.default_shape_cfg.kd = 1.0e1 + builder.default_shape_cfg.mu = 1.0 + + anchor_pos = wp.vec3(0.0, 0.0, 3.0) + + # Build a straight cable along +X and use its segment orientation for the kinematic anchor. + num_elements = 20 + segment_length = 0.05 + points, edge_q = _make_straight_cable_along_x(num_elements, segment_length, z_height=anchor_pos[2]) + anchor_rot = edge_q[0] + + # Kinematic anchor body at the rod start point (match rod orientation). + anchor = builder.add_body(xform=wp.transform(anchor_pos, anchor_rot)) + builder.body_mass[anchor] = 0.0 + builder.body_inv_mass[anchor] = 0.0 + # Anchor marker sphere. + anchor_radius = 0.1 + builder.add_shape_sphere(anchor, radius=anchor_radius) + + rod_radius = 0.01 + # Attach the cable endpoint to the sphere surface (not the center), accounting for cable radius so the + # capsule endcap surface and the sphere surface are coincident along the rod axis (+X). + # The rod axis is world +X, and anchor_rot maps parent-local +Z -> world +X, so use +Z in parent local. + attach_offset = wp.float32(anchor_radius + rod_radius) + parent_anchor_local = wp.vec3(0.0, 0.0, attach_offset) + anchor_world_attach = anchor_pos + wp.quat_rotate(anchor_rot, parent_anchor_local) + + # Reposition the generated cable so its first point coincides with the sphere-surface attach point. + p0 = points[0] + offset = anchor_world_attach - p0 + points = [p + offset for p in points] + + rod_bodies, rod_joints = builder.add_rod( + positions=points, + quaternions=edge_q, + radius=rod_radius, + bend_stiffness=1.0e-1, + bend_damping=1.0e-2, + stretch_stiffness=1.0e9, + stretch_damping=0.0, + wrap_in_articulation=False, + key="test_cable_fixed_joint_attach", + ) + + child_anchor_local = wp.vec3(0.0, 0.0, 0.0) + j_fixed = builder.add_joint_fixed( + parent=anchor, + child=rod_bodies[0], + parent_xform=wp.transform(parent_anchor_local, wp.quat_identity()), + child_xform=wp.transform(child_anchor_local, wp.quat_identity()), + ) + builder.add_articulation([*rod_joints, j_fixed]) + + builder.add_ground_plane() + builder.color() + model = builder.finalize(device=device) + model.set_gravity((0.0, 0.0, -9.81)) + + state0 = model.state() + state1 = model.state() + control = model.control() + + # Stiffen both linear and angular caps for non-cable joints so FIXED behaves near-hard. + solver = newton.solvers.SolverVBD( + model, + iterations=10, + rigid_joint_linear_ke=1.0e9, + rigid_joint_angular_ke=1.0e9, + rigid_joint_linear_k_start=1.0e7, + rigid_joint_angular_k_start=1.0e7, + ) + + frame_dt = 1.0 / 60.0 + sim_substeps = 10 + sim_dt = frame_dt / sim_substeps + num_steps = 20 + + for _step in range(num_steps): + for _substep in range(sim_substeps): + t = (_step * sim_substeps + _substep) * sim_dt + # Use wp.float32 so Warp builtins match expected scalar types (avoid numpy.float64). + dx = wp.float32(0.05 * np.sin(1.5 * t)) + ang = wp.float32(0.4 * np.sin(1.5 * t + 0.7)) + q_drive = wp.quat_from_axis_angle(wp.vec3(0.0, 1.0, 0.0), ang) + q = wp.mul(q_drive, anchor_rot) + + pose = wp.transform(wp.vec3(dx, 0.0, anchor_pos[2]), q) + wp.launch( + _set_kinematic_body_pose, + dim=1, + inputs=[wp.int32(anchor), pose, state0.body_q, state0.body_qd], + device=device, + ) + + contacts = model.collide(state0) + solver.step(state0, state1, control, contacts, dt=sim_dt) + state0, state1 = state1, state0 + + pos_err, ang_err = _compute_fixed_joint_frame_error(model, state0.body_q, j_fixed) + test.assertLess(pos_err, 1.0e-3) + test.assertLess(ang_err, 2.0e-2) + + final_q = state0.body_q.numpy() + test.assertTrue(np.isfinite(final_q).all(), "Non-finite body transforms detected in FIXED joint test") + _assert_surface_attachment( + test, + body_q=final_q, + anchor_body=anchor, + child_body=rod_bodies[0], + context="Cable FIXED joint attachment", + parent_anchor_local=parent_anchor_local, + ) + + _assert_bodies_above_ground( + test, + body_q=final_q, + body_ids=rod_bodies, + margin=0.25 * segment_length, + context="Cable FIXED joint attachment", + ) + _assert_capsule_attachments( + test, + body_q=final_q, + body_ids=rod_bodies, + segment_length=segment_length, + context="Cable FIXED joint attachment", + ) + + class TestCable(unittest.TestCase): pass @@ -731,6 +1195,18 @@ class TestCable(unittest.TestCase): _two_layer_cable_pile_collision_impl, devices=devices, ) +add_function_test( + TestCable, + "test_cable_ball_joint_attaches_rod_endpoint", + _cable_ball_joint_attaches_rod_endpoint_impl, + devices=devices, +) +add_function_test( + TestCable, + "test_cable_fixed_joint_attaches_rod_endpoint", + _cable_fixed_joint_attaches_rod_endpoint_impl, + devices=devices, +) if __name__ == "__main__": unittest.main(verbosity=2, failfast=True)