diff --git a/CHANGELOG.md b/CHANGELOG.md index d9a0cc9321..965d78bf22 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -33,6 +33,8 @@ - Add `total_force_friction` and `force_matrix_friction` to `SensorContact` for tangential (friction) force decomposition - Add `compute_normals` and `compute_uvs` optional arguments to `Mesh.create_heightfield()` and `Mesh.create_terrain()` - Add RJ45 plug-socket insertion example with SDF contacts, latch joint, and interactive gizmo +- Add `joint_damping` model attribute and `JointDofConfig.damping` for velocity-proportional damping that is always active + ### Changed @@ -119,6 +121,8 @@ - Fix heightfield bounding-sphere radius underestimating Z extent for asymmetric height ranges (e.g. `min_z=0, max_z=10`) - Fix VBD self-contact barrier C2 discontinuity at `d = tau` caused by a factor-of-two error in the log-barrier coefficient - Fix fast inertia validation treating near-symmetric tensors within `np.allclose()` default tolerances as corrections, aligning CPU and GPU validation warnings +- Fix MJCF joint `damping` attribute being ignored by `SolverFeatherstone` + ## [1.0.0] - 2026-03-10 diff --git a/newton/_src/sim/builder.py b/newton/_src/sim/builder.py index f150c4aae3..95243f4a50 100644 --- a/newton/_src/sim/builder.py +++ b/newton/_src/sim/builder.py @@ -432,6 +432,7 @@ def __init__( target_vel: float = 0.0, target_ke: float = 0.0, target_kd: float = 0.0, + damping: float = 0.0, armature: float = 0.0, effort_limit: float = 1e6, velocity_limit: float = 1e6, @@ -458,6 +459,8 @@ def __init__( """The proportional gain of the target drive PD controller. Defaults to 0.0.""" self.target_kd = target_kd """The derivative gain of the target drive PD controller. Defaults to 0.0.""" + self.damping = damping + """Passive velocity damping [N·s/m or N·m·s/rad, depending on joint type] that is always active. Defaults to 0.0.""" self.armature = armature """Artificial inertia added around the joint axis [kg·m² or kg]. Defaults to 0.""" self.effort_limit = effort_limit @@ -484,6 +487,7 @@ def create_unlimited(cls, axis: AxisType | Vec3) -> ModelBuilder.JointDofConfig: target_vel=0.0, target_ke=0.0, target_kd=0.0, + damping=0.0, armature=0.0, limit_ke=0.0, limit_kd=0.0, @@ -1070,6 +1074,8 @@ def __init__(self, up_axis: AxisType = Axis.Z, gravity: float = -9.81): """Joint target stiffness values accumulated for :attr:`Model.joint_target_ke`.""" self.joint_target_kd: list[float] = [] """Joint target damping values accumulated for :attr:`Model.joint_target_kd`.""" + self.joint_damping: list[float] = [] + """Passive velocity damping values accumulated for :attr:`Model.joint_damping`.""" self.joint_limit_lower: list[float] = [] """Lower joint limits accumulated for :attr:`Model.joint_limit_lower`.""" self.joint_limit_upper: list[float] = [] @@ -3071,6 +3077,7 @@ def transform_mul(a: wp.transform, b: wp.transform) -> wp.transform: "joint_limit_kd", "joint_target_ke", "joint_target_kd", + "joint_damping", "joint_target_mode", "joint_effort_limit", "joint_velocity_limit", @@ -3624,6 +3631,7 @@ def add_axis_dim(dim: ModelBuilder.JointDofConfig): self.joint_target_mode.append(mode) self.joint_target_ke.append(dim.target_ke) self.joint_target_kd.append(dim.target_kd) + self.joint_damping.append(dim.damping) self.joint_limit_ke.append(dim.limit_ke) self.joint_limit_kd.append(dim.limit_kd) self.joint_armature.append(dim.armature) @@ -3699,6 +3707,7 @@ def add_joint_revolute( target_vel: float | None = None, target_ke: float | None = None, target_kd: float | None = None, + damping: float | None = None, limit_lower: float | None = None, limit_upper: float | None = None, limit_ke: float | None = None, @@ -3728,6 +3737,7 @@ def add_joint_revolute( target_vel: The target velocity of the joint. target_ke: The stiffness of the joint target. target_kd: The damping of the joint target. + damping: Passive velocity damping [N·s/m or N·m·s/rad, depending on joint type] always active on the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.damping`` is used. limit_lower: The lower limit of the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_lower`` is used. limit_upper: The upper limit of the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_upper`` is used. limit_ke: The stiffness of the joint limit. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_ke`` is used. @@ -3759,6 +3769,7 @@ def add_joint_revolute( target_vel=target_vel if target_vel is not None else self.default_joint_cfg.target_vel, target_ke=target_ke if target_ke is not None else self.default_joint_cfg.target_ke, target_kd=target_kd if target_kd is not None else self.default_joint_cfg.target_kd, + damping=damping if damping is not None else self.default_joint_cfg.damping, limit_ke=limit_ke if limit_ke is not None else self.default_joint_cfg.limit_ke, limit_kd=limit_kd if limit_kd is not None else self.default_joint_cfg.limit_kd, armature=armature if armature is not None else self.default_joint_cfg.armature, @@ -3792,6 +3803,7 @@ def add_joint_prismatic( target_vel: float | None = None, target_ke: float | None = None, target_kd: float | None = None, + damping: float | None = None, limit_lower: float | None = None, limit_upper: float | None = None, limit_ke: float | None = None, @@ -3820,6 +3832,7 @@ def add_joint_prismatic( target_vel: The target velocity of the joint. target_ke: The stiffness of the joint target. target_kd: The damping of the joint target. + damping: Passive velocity damping [N·s/m or N·m·s/rad, depending on joint type] always active on the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.damping`` is used. limit_lower: The lower limit of the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_lower`` is used. limit_upper: The upper limit of the joint. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_upper`` is used. limit_ke: The stiffness of the joint limit. If None, the default value from ``ModelBuilder.default_joint_cfg.limit_ke`` is used. @@ -3851,6 +3864,7 @@ def add_joint_prismatic( target_vel=target_vel if target_vel is not None else self.default_joint_cfg.target_vel, target_ke=target_ke if target_ke is not None else self.default_joint_cfg.target_ke, target_kd=target_kd if target_kd is not None else self.default_joint_cfg.target_kd, + damping=damping if damping is not None else self.default_joint_cfg.damping, limit_ke=limit_ke if limit_ke is not None else self.default_joint_cfg.limit_ke, limit_kd=limit_kd if limit_kd is not None else self.default_joint_cfg.limit_kd, armature=armature if armature is not None else self.default_joint_cfg.armature, @@ -4701,6 +4715,7 @@ def collapse_fixed_joints( "actuator_mode": self.joint_target_mode[j], "target_ke": self.joint_target_ke[j], "target_kd": self.joint_target_kd[j], + "damping": self.joint_damping[j], "limit_ke": self.joint_limit_ke[j], "limit_kd": self.joint_limit_kd[j], "limit_lower": self.joint_limit_lower[j], @@ -4977,6 +4992,7 @@ def dfs(parent_body: int, child_body: int, incoming_xform: wp.transform, last_dy self.joint_target_mode.clear() self.joint_target_ke.clear() self.joint_target_kd.clear() + self.joint_damping.clear() self.joint_limit_lower.clear() self.joint_limit_upper.clear() self.joint_limit_ke.clear() @@ -5019,6 +5035,7 @@ def dfs(parent_body: int, child_body: int, incoming_xform: wp.transform, last_dy self.joint_target_mode.append(axis["actuator_mode"]) self.joint_target_ke.append(axis["target_ke"]) self.joint_target_kd.append(axis["target_kd"]) + self.joint_damping.append(axis["damping"]) self.joint_limit_lower.append(axis["limit_lower"]) self.joint_limit_upper.append(axis["limit_upper"]) self.joint_limit_ke.append(axis["limit_ke"]) @@ -9107,6 +9124,7 @@ def _validate_structure(self) -> None: ("joint_armature", self.joint_armature), ("joint_target_ke", self.joint_target_ke), ("joint_target_kd", self.joint_target_kd), + ("joint_damping", self.joint_damping), ("joint_limit_lower", self.joint_limit_lower), ("joint_limit_upper", self.joint_limit_upper), ("joint_limit_ke", self.joint_limit_ke), @@ -10176,6 +10194,7 @@ def _to_wp_array(data, dtype, requires_grad): m.joint_target_mode = wp.array(self.joint_target_mode, dtype=wp.int32) m.joint_target_ke = wp.array(self.joint_target_ke, dtype=wp.float32, requires_grad=requires_grad) m.joint_target_kd = wp.array(self.joint_target_kd, dtype=wp.float32, requires_grad=requires_grad) + m.joint_damping = wp.array(self.joint_damping, dtype=wp.float32, requires_grad=requires_grad) m.joint_target_pos = wp.array(self.joint_target_pos, dtype=wp.float32, requires_grad=requires_grad) m.joint_target_vel = wp.array(self.joint_target_vel, dtype=wp.float32, requires_grad=requires_grad) m.joint_f = wp.array(self.joint_f, dtype=wp.float32, requires_grad=requires_grad) diff --git a/newton/_src/sim/model.py b/newton/_src/sim/model.py index a7539c66fb..17141897aa 100644 --- a/newton/_src/sim/model.py +++ b/newton/_src/sim/model.py @@ -455,6 +455,8 @@ def __init__(self, device: Devicelike | None = None): """Joint stiffness [N/m or N·m/rad, depending on joint type], shape [joint_dof_count], float.""" self.joint_target_kd: wp.array[wp.float32] | None = None """Joint damping [N·s/m or N·m·s/rad, depending on joint type], shape [joint_dof_count], float.""" + self.joint_damping: wp.array[wp.float32] | None = None + """Passive velocity damping [N·s/m or N·m·s/rad, depending on joint type] always active on the joint, shape [joint_dof_count], float.""" self.joint_effort_limit: wp.array[wp.float32] | None = None """Joint effort (force/torque) limits [N or N·m, depending on joint type], shape [joint_dof_count], float.""" self.joint_velocity_limit: wp.array[wp.float32] | None = None @@ -763,6 +765,7 @@ def __init__(self, device: Devicelike | None = None): self.attribute_frequency["joint_target_mode"] = Model.AttributeFrequency.JOINT_DOF self.attribute_frequency["joint_target_ke"] = Model.AttributeFrequency.JOINT_DOF self.attribute_frequency["joint_target_kd"] = Model.AttributeFrequency.JOINT_DOF + self.attribute_frequency["joint_damping"] = Model.AttributeFrequency.JOINT_DOF self.attribute_frequency["joint_limit_lower"] = Model.AttributeFrequency.JOINT_DOF self.attribute_frequency["joint_limit_upper"] = Model.AttributeFrequency.JOINT_DOF self.attribute_frequency["joint_limit_ke"] = Model.AttributeFrequency.JOINT_DOF diff --git a/newton/_src/solvers/featherstone/kernels.py b/newton/_src/solvers/featherstone/kernels.py index 5904bf33ae..588c3f75a3 100644 --- a/newton/_src/solvers/featherstone/kernels.py +++ b/newton/_src/solvers/featherstone/kernels.py @@ -346,6 +346,7 @@ def jcalc_tau( joint_target_kd: wp.array[float], joint_limit_ke: wp.array[float], joint_limit_kd: wp.array[float], + joint_damping: wp.array[float], joint_S_s: wp.array[wp.spatial_vector], joint_q: wp.array[float], joint_qd: wp.array[float], @@ -402,8 +403,11 @@ def jcalc_tau( target_kd = joint_target_kd[j] target_pos = joint_target_pos[j] target_vel = joint_target_vel[j] + damping = joint_damping[j] - drive_f = joint_force(q, qd, target_pos, target_vel, target_ke, target_kd, lower, upper, limit_ke, limit_kd) + drive_f = joint_force( + q, qd, target_pos, target_vel, target_ke, target_kd, lower, upper, limit_ke, limit_kd, damping + ) # total torque / force on the joint t = -wp.dot(S_s, body_f_s) + drive_f + joint_f[j] @@ -853,6 +857,7 @@ def eval_rigid_tau( joint_limit_upper: wp.array[float], joint_limit_ke: wp.array[float], joint_limit_kd: wp.array[float], + joint_damping: wp.array[float], joint_S_s: wp.array[wp.spatial_vector], body_fb_s: wp.array[wp.spatial_vector], body_f_ext: wp.array[wp.spatial_vector], @@ -893,6 +898,7 @@ def eval_rigid_tau( joint_target_kd, joint_limit_ke, joint_limit_kd, + joint_damping, joint_S_s, joint_q, joint_qd, diff --git a/newton/_src/solvers/featherstone/solver_featherstone.py b/newton/_src/solvers/featherstone/solver_featherstone.py index 3dc6389742..80268927a6 100644 --- a/newton/_src/solvers/featherstone/solver_featherstone.py +++ b/newton/_src/solvers/featherstone/solver_featherstone.py @@ -511,6 +511,7 @@ def step( model.joint_limit_upper, model.joint_limit_ke, model.joint_limit_kd, + model.joint_damping, state_aug.joint_S_s, state_aug.body_f_s, body_f, diff --git a/newton/_src/solvers/flags.py b/newton/_src/solvers/flags.py index 7e375e983e..42992ca5fe 100644 --- a/newton/_src/solvers/flags.py +++ b/newton/_src/solvers/flags.py @@ -19,7 +19,7 @@ class SolverNotifyFlags(IntEnum): """Indicates joint property updates: joint_q, joint_X_p, joint_X_c.""" JOINT_DOF_PROPERTIES = 1 << 1 - """Indicates joint DOF property updates: joint_target_ke, joint_target_kd, joint_effort_limit, joint_armature, joint_friction, joint_limit_ke, joint_limit_kd, joint_limit_lower, joint_limit_upper.""" + """Indicates joint DOF property updates: joint_target_ke, joint_target_kd, joint_damping, joint_effort_limit, joint_armature, joint_friction, joint_limit_ke, joint_limit_kd, joint_limit_lower, joint_limit_upper.""" BODY_PROPERTIES = 1 << 2 """Indicates body property updates: body_q, body_qd, body_flags.""" diff --git a/newton/_src/solvers/semi_implicit/kernels_body.py b/newton/_src/solvers/semi_implicit/kernels_body.py index 8cd7a41b00..ada4d63292 100644 --- a/newton/_src/solvers/semi_implicit/kernels_body.py +++ b/newton/_src/solvers/semi_implicit/kernels_body.py @@ -26,6 +26,7 @@ def joint_force( limit_upper: float, limit_ke: float, limit_kd: float, + damping: float, ) -> float: """Joint force evaluation for a single degree of freedom.""" @@ -45,7 +46,9 @@ def joint_force( damping_f = -limit_kd * qd target_f = 0.0 - return limit_f + damping_f + target_f + passive_f = -damping * qd + + return limit_f + damping_f + target_f + passive_f @wp.kernel @@ -71,6 +74,7 @@ def eval_body_joints( joint_limit_upper: wp.array[float], joint_limit_ke: wp.array[float], joint_limit_kd: wp.array[float], + joint_damping: wp.array[float], joint_attach_ke: float, joint_attach_kd: float, body_f: wp.array[wp.spatial_vector], @@ -178,6 +182,7 @@ def eval_body_joints( joint_limit_upper[qd_start], joint_limit_ke[qd_start], joint_limit_kd[qd_start], + joint_damping[qd_start], ) ) @@ -215,6 +220,7 @@ def eval_body_joints( joint_limit_upper[qd_start], joint_limit_ke[qd_start], joint_limit_kd[qd_start], + joint_damping[qd_start], ) ) @@ -254,6 +260,7 @@ def eval_body_joints( joint_limit_upper[qd_start + 0], joint_limit_ke[qd_start + 0], joint_limit_kd[qd_start + 0], + joint_damping[qd_start + 0], ) ) @@ -278,6 +285,7 @@ def eval_body_joints( joint_limit_upper[qd_start + 1], joint_limit_ke[qd_start + 1], joint_limit_kd[qd_start + 1], + joint_damping[qd_start + 1], ) ) @@ -302,6 +310,7 @@ def eval_body_joints( joint_limit_upper[qd_start + 2], joint_limit_ke[qd_start + 2], joint_limit_kd[qd_start + 2], + joint_damping[qd_start + 2], ) ) @@ -346,6 +355,7 @@ def eval_body_joints( joint_limit_upper[i_0], joint_limit_ke[i_0], joint_limit_kd[i_0], + joint_damping[i_0], ) ) @@ -392,6 +402,7 @@ def eval_body_joints( joint_limit_upper[i_0], joint_limit_ke[i_0], joint_limit_kd[i_0], + joint_damping[i_0], ) ) t_total += axis_1 * ( @@ -407,6 +418,7 @@ def eval_body_joints( joint_limit_upper[i_1], joint_limit_ke[i_1], joint_limit_kd[i_1], + joint_damping[i_1], ) ) @@ -422,6 +434,7 @@ def eval_body_joints( 0.0, 0.0, 0.0, + 0.0, ) if ang_axis_count == 3: @@ -460,6 +473,7 @@ def eval_body_joints( joint_limit_upper[i_0], joint_limit_ke[i_0], joint_limit_kd[i_0], + joint_damping[i_0], ) ) t_total += axis_1 * ( @@ -475,6 +489,7 @@ def eval_body_joints( joint_limit_upper[i_1], joint_limit_ke[i_1], joint_limit_kd[i_1], + joint_damping[i_1], ) ) t_total += axis_2 * ( @@ -490,6 +505,7 @@ def eval_body_joints( joint_limit_upper[i_2], joint_limit_ke[i_2], joint_limit_kd[i_2], + joint_damping[i_2], ) ) @@ -529,6 +545,7 @@ def eval_body_joint_forces( model.joint_limit_upper, model.joint_limit_ke, model.joint_limit_kd, + model.joint_damping, joint_attach_ke, joint_attach_kd, ], diff --git a/newton/_src/utils/import_mjcf.py b/newton/_src/utils/import_mjcf.py index 66b0588ca0..f755b23a13 100644 --- a/newton/_src/utils/import_mjcf.py +++ b/newton/_src/utils/import_mjcf.py @@ -317,6 +317,7 @@ def parse_mjcf( default_joint_limit_upper = builder.default_joint_cfg.limit_upper default_joint_target_ke = builder.default_joint_cfg.target_ke default_joint_target_kd = builder.default_joint_cfg.target_kd + default_joint_damping = builder.default_joint_cfg.damping default_joint_armature = builder.default_joint_cfg.armature default_joint_effort_limit = builder.default_joint_cfg.effort_limit @@ -1548,6 +1549,7 @@ def parse_body( limit_kd=limit_kd, target_ke=default_joint_target_ke, target_kd=default_joint_target_kd, + damping=parse_float(joint_attrib, "damping", default_joint_damping), armature=joint_armature[-1], friction=parse_float(joint_attrib, "frictionloss", 0.0), effort_limit=effort_limit, diff --git a/newton/tests/test_import_mjcf.py b/newton/tests/test_import_mjcf.py index a92866412a..e263a7875c 100644 --- a/newton/tests/test_import_mjcf.py +++ b/newton/tests/test_import_mjcf.py @@ -3199,6 +3199,7 @@ def test_joint_stiffness_damping(self): joint_damping = model.mujoco.dof_passive_damping.numpy() joint_target_ke = model.joint_target_ke.numpy() joint_target_kd = model.joint_target_kd.numpy() + joint_damping = model.joint_damping.numpy() prefix = "stiffness_damping_comprehensive_test/worldbody" expected_values = { @@ -3213,6 +3214,7 @@ def test_joint_stiffness_damping(self): dof_idx = joint_qd_start[joint_idx] self.assertAlmostEqual(joint_stiffness[dof_idx], expected["stiffness"], places=4) self.assertAlmostEqual(joint_damping[dof_idx], expected["damping"], places=4) + self.assertAlmostEqual(joint_damping[dof_idx], expected["damping"], places=4) self.assertAlmostEqual(joint_target_ke[dof_idx], expected["target_ke"], places=1) self.assertAlmostEqual(joint_target_kd[dof_idx], expected["target_kd"], places=1)