diff --git a/newton/_src/sim/builder.py b/newton/_src/sim/builder.py index 37f2fa088..588664fca 100644 --- a/newton/_src/sim/builder.py +++ b/newton/_src/sim/builder.py @@ -399,6 +399,13 @@ def __init__(self, up_axis: AxisType = Axis.Z, gravity: float = -9.81): self.muscle_bodies = [] self.muscle_points = [] + # tendons (fixed) + self.tendon_start = [] # start index in tendon_joints for each tendon + self.tendon_params = [] # (stiffness, damping, rest_length, lower_limit, upper_limit) for each tendon + self.tendon_joints = [] # joint indices attached to tendons + self.tendon_gearings = [] # gearing coefficients for each joint attachment + self.tendon_key = [] # string key for each tendon + # rigid bodies self.body_mass = [] self.body_inertia = [] @@ -1174,11 +1181,31 @@ def transform_mul(a, b): "equality_constraint_polycoef", "equality_constraint_key", "equality_constraint_enabled", + "tendon_params", + "tendon_key", ] for attr in more_builder_attrs: getattr(self, attr).extend(getattr(builder, attr)) + # Handle tendon copying with proper joint indices offset + if builder.tendon_start: + # The joint offset to apply to tendon joint references + joint_offset = self.joint_count - builder.joint_count + + # Update tendon_start indices to account for existing tendons + existing_tendon_joints = len(self.tendon_joints) + for i in range(len(builder.tendon_start)): + if i == 0: + self.tendon_start.append(existing_tendon_joints) + else: + self.tendon_start.append(existing_tendon_joints + builder.tendon_start[i]) + + # Copy tendon_joints with offset and tendon_gearings as-is + for joint_idx in builder.tendon_joints: + self.tendon_joints.append(joint_idx + joint_offset) + self.tendon_gearings.extend(builder.tendon_gearings) + self.joint_dof_count += builder.joint_dof_count self.joint_coord_count += builder.joint_coord_count @@ -2380,6 +2407,59 @@ def add_muscle( # return the index of the muscle return len(self.muscle_start) - 1 + def add_tendon( + self, + name: str, + joint_ids: list[int], + gearings: list[float], + stiffness: float = 0.0, + damping: float = 0.0, + rest_length: float = 0.0, + lower_limit: float = float("-inf"), + upper_limit: float = float("inf"), + ) -> int: + """Adds a fixed tendon constraint between multiple joints. + + Fixed tendons couple the motion of multiple joints through a linear + constraint on their positions. The tendon length is computed as: + L = rest_length + sum(gearing[i] * joint_pos[i]) + + Args: + name: A unique identifier for the tendon + joint_ids: List of joint indices that this tendon connects + gearings: Gearing coefficient for each joint (transmission ratio) + stiffness: Elastic stiffness of the tendon (0 for hard constraint) + damping: Damping coefficient + rest_length: Rest length of the tendon + lower_limit: Lower limit for tendon length + upper_limit: Upper limit for tendon length + + Returns: + The index of the tendon in the model + """ + if len(joint_ids) != len(gearings): + raise ValueError("Number of joint IDs must match number of gearings") + + if len(joint_ids) < 2: + raise ValueError("Tendon must connect at least 2 joints") + + # Store the start index for this tendon's joint data + self.tendon_start.append(len(self.tendon_joints)) + + # Store tendon parameters + self.tendon_params.append((stiffness, damping, rest_length, lower_limit, upper_limit)) + + # Store the name/key + self.tendon_key.append(name) + + # Store joint indices and gearings + for joint_id, gearing in zip(joint_ids, gearings, strict=False): + self.tendon_joints.append(joint_id) + self.tendon_gearings.append(gearing) + + # Return the index of the tendon + return len(self.tendon_start) - 1 + # region shapes def add_shape( @@ -4230,6 +4310,19 @@ def finalize(self, device: Devicelike | None = None, requires_grad: bool = False m.muscle_points = wp.array(self.muscle_points, dtype=wp.vec3, requires_grad=requires_grad) m.muscle_activations = wp.array(self.muscle_activations, dtype=wp.float32, requires_grad=requires_grad) + # ----------------------- + # tendons + + # close the tendon joint indices + tendon_start = copy.copy(self.tendon_start) + tendon_start.append(len(self.tendon_joints)) + + m.tendon_start = wp.array(tendon_start, dtype=wp.int32) + m.tendon_params = wp.array(self.tendon_params, dtype=wp.float32, requires_grad=requires_grad) + m.tendon_joints = wp.array(self.tendon_joints, dtype=wp.int32) + m.tendon_gearings = wp.array(self.tendon_gearings, dtype=wp.float32, requires_grad=requires_grad) + m.tendon_key = self.tendon_key + # -------------------------------------- # rigid bodies @@ -4400,6 +4493,7 @@ def finalize(self, device: Devicelike | None = None, requires_grad: bool = False m.edge_count = len(self.edge_rest_angle) m.spring_count = len(self.spring_rest_length) m.muscle_count = len(self.muscle_start) + m.tendon_count = len(self.tendon_start) m.articulation_count = len(self.articulation_start) m.equality_constraint_count = len(self.equality_constraint_type) diff --git a/newton/_src/sim/model.py b/newton/_src/sim/model.py index b2b0c7670..df50900d3 100644 --- a/newton/_src/sim/model.py +++ b/newton/_src/sim/model.py @@ -205,6 +205,17 @@ def __init__(self, device: Devicelike | None = None): self.muscle_activations = None """Muscle activations, shape [muscle_count], float.""" + self.tendon_start = None + """Start indices for each tendon's joint attachments, shape [tendon_count + 1], int.""" + self.tendon_params = None + """Tendon parameters (stiffness, damping, rest_length, lower_limit, upper_limit), shape [tendon_count, 5], float.""" + self.tendon_joints = None + """Joint indices that tendons attach to, int.""" + self.tendon_gearings = None + """Gearing coefficients for each joint attachment, float.""" + self.tendon_key = [] + """Tendon keys/names, shape [tendon_count], str.""" + self.body_q = None """Rigid body poses for state initialization, shape [body_count, 7], float.""" self.body_qd = None @@ -357,6 +368,8 @@ def __init__(self, device: Devicelike | None = None): """Total number of springs in the system.""" self.muscle_count = 0 """Total number of muscles in the system.""" + self.tendon_count = 0 + """Total number of tendons in the system.""" self.articulation_count = 0 """Total number of articulations in the system.""" self.joint_dof_count = 0 diff --git a/newton/_src/solvers/mujoco/solver_mujoco.py b/newton/_src/solvers/mujoco/solver_mujoco.py index 0d827771f..a81101151 100644 --- a/newton/_src/solvers/mujoco/solver_mujoco.py +++ b/newton/_src/solvers/mujoco/solver_mujoco.py @@ -2240,6 +2240,93 @@ def add_geoms(newton_body_id: int, incoming_xform: wp.transform | None = None): eq.data[6:10] = wp.transform_get_rotation(eq_constraint_relpose[i]) eq.data[10] = eq_constraint_torquescale[i] + # Add fixed tendons from Newton model + if hasattr(model, "tendon_count") and model.tendon_count > 0: + # Access tendon data arrays on host + tendon_start = model.tendon_start.numpy() if hasattr(model.tendon_start, "numpy") else model.tendon_start + tendon_params = ( + model.tendon_params.numpy() if hasattr(model.tendon_params, "numpy") else model.tendon_params + ) + tendon_joints = ( + model.tendon_joints.numpy() if hasattr(model.tendon_joints, "numpy") else model.tendon_joints + ) + tendon_gearings = ( + model.tendon_gearings.numpy() if hasattr(model.tendon_gearings, "numpy") else model.tendon_gearings + ) + + # Track tendon names to ensure uniqueness across environments + tendon_name_counts = {} + + # Add each tendon + for i in range(model.tendon_count): + # Get the joints and gearings for this tendon + start_idx = tendon_start[i] + end_idx = tendon_start[i + 1] + + # Check if all joints in this tendon are in the selected set + # Only add tendons where all joints are selected + all_joints_selected = True + for j in range(start_idx, end_idx): + joint_idx = tendon_joints[j] + if joint_idx not in selected_joints: + all_joints_selected = False + break + + # Skip this tendon if not all joints are selected + if not all_joints_selected: + continue + + # Create fixed tendon + tendon = spec.add_tendon() + + # Ensure unique tendon name (similar to body naming) + base_name = model.tendon_key[i] if i < len(model.tendon_key) else f"tendon_{i}" + name = base_name + if name not in tendon_name_counts: + tendon_name_counts[name] = 1 + else: + tendon_name_counts[name] += 1 + name = f"{base_name}_{tendon_name_counts[name]}" + + tendon.name = name + + # Get tendon parameters (stiffness, damping, rest_length, lower_limit, upper_limit) + params = tendon_params[i] + stiffness = params[0] + damping = params[1] + rest_length = params[2] + lower_limit = params[3] + upper_limit = params[4] + + # Set tendon properties + # In MuJoCo, fixed tendons use the following fields: + # - springlength: rest length of the tendon (needs to be array [2, 1]) + # - stiffness: elastic stiffness + # - damping: damping coefficient + # - limited: whether limits are enabled + # - range: [lower_limit, upper_limit] + # springlength needs to be a 2x1 array for MuJoCo spec API + springlength_array = np.array([[rest_length], [0.0]], dtype=np.float64) + tendon.springlength = springlength_array + tendon.stiffness = stiffness + tendon.damping = damping + + # Set limits if they are finite + if not (np.isinf(lower_limit) or np.isinf(upper_limit)): + tendon.limited = True + tendon.range[0] = lower_limit + tendon.range[1] = upper_limit + + # Add joints to the fixed tendon + for j in range(start_idx, end_idx): + joint_idx = tendon_joints[j] + gearing = tendon_gearings[j] + joint_name = model.joint_key[joint_idx] + + # In MuJoCo spec API, use wrap_joint to add a joint to the tendon + # This is the Python equivalent of mjs_wrapJoint in C API + tendon.wrap_joint(joint_name, gearing) + assert len(spec.geoms) == colliding_shapes_per_env, ( "The number of geoms in the MuJoCo model does not match the number of colliding shapes in the Newton model." ) diff --git a/newton/_src/utils/import_usd.py b/newton/_src/utils/import_usd.py index c2933918e..3cf7248e0 100644 --- a/newton/_src/utils/import_usd.py +++ b/newton/_src/utils/import_usd.py @@ -32,6 +32,41 @@ from ..sim.joints import JointMode +def extract_tendon_info_from_joint(joint_prim): + """ + Extract PhysX tendon information from a joint prim. + + Returns: + dict: Dictionary with tendon names as keys and dict containing: + - 'is_root': bool - whether this joint is the tendon root + - 'attrs': dict - tendon attributes + """ + tendon_info = {} + + # Use Newton's existing pattern for API schemas + api_schemas = joint_prim.GetPrimTypeInfo().GetAppliedAPISchemas() + + # Find all tendon-related schemas (both root and participant) + for schema in api_schemas: + if "PhysxTendonAxisRootAPI:" in schema or "PhysxTendonAxisAPI:" in schema: + # Extract tendon name from schema + tendon_name = schema.split(":", 1)[1] + is_root = "Root" in schema + + # Get tendon attributes + tendon_attrs = {} + for attr in joint_prim.GetAttributes(): + attr_name = attr.GetName() + if f"physxTendon:{tendon_name}:" in attr_name: + # Extract attribute type + attr_type = attr_name.split(":")[-1] + tendon_attrs[attr_type] = attr.Get() + + tendon_info[tendon_name] = {"is_root": is_root, "attrs": tendon_attrs} + + return tendon_info + + def parse_usd( builder: ModelBuilder, source, @@ -329,8 +364,8 @@ def load_visual_shapes(parent_body_id, prim, incoming_xform): if path_name not in path_shape_map: if type_name == "cube": size = parse_float(prim, "size", 2.0) - if has_attribute(prim, "extents"): - extents = parse_vec(prim, "extents") * scale + if has_attribute(prim, "extent"): + extents = parse_vec(prim, "extent") * scale # TODO position geom at extents center? # geo_pos = 0.5 * (extents[0] + extents[1]) extents = extents[1] - extents[0] @@ -515,7 +550,7 @@ def parse_scale(prim): def parse_joint(joint_desc, joint_path, incoming_xform=None): if not joint_desc.jointEnabled and only_load_enabled_joints: - return + return None key = joint_desc.type joint_prim = stage.GetPrimAtPath(joint_desc.primPath) parent_path = str(joint_desc.body0) @@ -717,6 +752,14 @@ def define_joint_mode(dof, joint_desc): else: raise NotImplementedError(f"Unsupported joint type {key}") + # Extract tendon information from this joint + tendon_info = extract_tendon_info_from_joint(joint_prim) + if verbose and tendon_info: + print(f" Joint {joint_path} has tendon info: {tendon_info}") + + # Return the joint ID and tendon info + return builder.joint_count - 1, tendon_info + # Looking for and parsing the attributes on PhysicsScene prims scene_attributes = {} if UsdPhysics.ObjectType.Scene in ret_dict: @@ -927,21 +970,96 @@ def data_for_key(physics_utils_results, key): child_body_id = art_bodies[first_joint_parent] builder.add_joint_free(child=child_body_id) builder.joint_q[-7:] = articulation_xform + + # Collect tendon information as we parse joints + tendon_data = {} + joint_path_to_id = {} + for joint_id, i in enumerate(sorted_joints): if joint_id == 0 and first_joint_parent == -1: # the articulation root joint receives the articulation transform as parent transform # except if we already inserted a floating-base joint - parse_joint( + joint_result = parse_joint( joint_descriptions[joint_names[i]], joint_path=joint_names[i], incoming_xform=articulation_xform, ) else: - parse_joint( + joint_result = parse_joint( joint_descriptions[joint_names[i]], joint_path=joint_names[i], ) + # Process tendon information if joint was created + if joint_result is not None: + created_joint_id, tendon_info = joint_result + joint_path_to_id[joint_names[i]] = created_joint_id + + # Collect tendon data + if tendon_info: + if verbose: + print(f" Processing tendon info from joint {joint_names[i]}: {tendon_info}") + for tendon_name, info in tendon_info.items(): + if tendon_name not in tendon_data: + tendon_data[tendon_name] = { + "joints": [], + "gearings": [], + "params": {}, + "root_joint": None, + } + + # Extract gearing coefficient + gearing = info["attrs"].get("gearing", [1.0]) + if hasattr(gearing, "__getitem__"): # Check if it's indexable (list or Vt.FloatArray) + gearing = float(gearing[0]) + else: + gearing = float(gearing) + + tendon_data[tendon_name]["joints"].append(created_joint_id) + tendon_data[tendon_name]["gearings"].append(gearing) + + # If this is the root joint, store parameters + if info["is_root"]: + tendon_data[tendon_name]["root_joint"] = joint_names[i] + for param in ["stiffness", "damping", "restLength", "lowerLimit", "upperLimit"]: + if param in info["attrs"]: + value = info["attrs"][param] + # Convert to float if needed + tendon_data[tendon_name]["params"][param] = float(value) + + if verbose: + print(f" Found tendon root: {tendon_name} on joint {joint_names[i]}") + else: + if verbose: + print(f" Found tendon participant: {tendon_name} on joint {joint_names[i]}") + + # Add tendons to builder after all joints are parsed + if verbose and tendon_data: + print(f"Found {len(tendon_data)} tendons to process") + for tendon_name, data in tendon_data.items(): + if len(data["joints"]) >= 2: + params = data["params"] + if hasattr(builder, "add_tendon"): + builder.add_tendon( + name=tendon_name, + joint_ids=data["joints"], + gearings=data["gearings"], + stiffness=params.get("stiffness", 0.0), + damping=params.get("damping", 0.0), + rest_length=params.get("restLength", 0.0), + lower_limit=params.get("lowerLimit", float("-inf")), + upper_limit=params.get("upperLimit", float("inf")), + ) + if verbose: + print(f"Added tendon {tendon_name} with {len(data['joints'])} joints") + else: + if verbose: + print( + f"Warning: ModelBuilder does not have add_tendon method, skipping tendon {tendon_name}" + ) + elif verbose: + print(f"Warning: Tendon {tendon_name} has only {len(data['joints'])} joint(s), skipping") + articulation_bodies[articulation_id] = art_bodies # determine if self-collisions are enabled articulation_has_self_collision[articulation_id] = parse_generic( @@ -1182,7 +1300,10 @@ def data_for_key(physics_utils_results, key): mass = parse_float(prim, "physics:mass") if mass is not None: builder.body_mass[body_id] = mass - builder.body_inv_mass[body_id] = 1.0 / mass + if mass > 0.0: + builder.body_inv_mass[body_id] = 1.0 / mass + else: + builder.body_inv_mass[body_id] = 0.0 com = parse_vec(prim, "physics:centerOfMass") if com is not None: builder.body_com[body_id] = com diff --git a/newton/examples/__init__.py b/newton/examples/__init__.py index 7bfe85bfc..4dc7151ba 100644 --- a/newton/examples/__init__.py +++ b/newton/examples/__init__.py @@ -199,6 +199,8 @@ def main(): "robot_g1": "newton.examples.robot.example_robot_g1", "robot_h1": "newton.examples.robot.example_robot_h1", "robot_humanoid": "newton.examples.robot.example_robot_humanoid", + "robot_shadowhand_tendon": "newton.examples.robot.example_robot_shadowhand_tendon", + "robot_simple_tendon": "newton.examples.robot.example_robot_simple_tendon", "selection_articulations": "newton.examples.selection.example_selection_articulations", "selection_cartpole": "newton.examples.selection.example_selection_cartpole", "selection_materials": "newton.examples.selection.example_selection_materials", diff --git a/newton/examples/assets/shadow_hand.usd b/newton/examples/assets/shadow_hand.usd new file mode 100644 index 000000000..ce1a048b8 Binary files /dev/null and b/newton/examples/assets/shadow_hand.usd differ diff --git a/newton/examples/robot/example_robot_shadowhand_tendon.py b/newton/examples/robot/example_robot_shadowhand_tendon.py new file mode 100644 index 000000000..173bfaf78 --- /dev/null +++ b/newton/examples/robot/example_robot_shadowhand_tendon.py @@ -0,0 +1,389 @@ +# 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 Robot Shadow Hand with Tendons +# +# Shows how to simulate the Shadow Hand robot with PhysX fixed tendons +# that couple finger joint movements. The tendons automatically handle +# the mechanical coupling between finger joints. +# +# Command: python -m newton.examples robot_shadowhand_tendon --num-envs 4 +# +########################################################################### + +import math + +import warp as wp + +import newton +import newton.examples +import newton.utils + + +class Example: + def __init__(self, viewer, num_envs=4, disable_tendons=True, mass_scale=1.0): + self.fps = 60 + self.frame_dt = 1.0 / self.fps + self.sim_time = 0.0 + self.sim_substeps = 10 # Increased from 4 for better stability + self.sim_dt = self.frame_dt / self.sim_substeps + + self.num_envs = num_envs + self.viewer = viewer + self.device = wp.get_device() + + # Build Shadow Hand model + hand = newton.ModelBuilder() + + # Configure joint and shape parameters + # Reduced stiffness values for stability + stiffness_scale = 0.01 # Scale all stiffnesses down + hand.default_joint_cfg = newton.ModelBuilder.JointDofConfig( + limit_ke=1.0e2 * stiffness_scale, + limit_kd=1.0e1 * stiffness_scale, + friction=1e-4, + armature=1e-2, # Increased armature for stability + ) + hand.default_shape_cfg.ke = 1.0e3 * stiffness_scale + hand.default_shape_cfg.kd = 1.0e2 * stiffness_scale + hand.default_shape_cfg.kf = 1.0e2 * stiffness_scale + hand.default_shape_cfg.mu = 0.75 + + # Also set body armature for additional stability + hand.default_body_armature = 1e-3 + + # Load Shadow Hand USD with tendons + # Using the USD file from the assets directory + asset_path = "newton/examples/assets/shadow_hand.usd" + + # Define joint ordering to avoid topological sort issues + # The Shadow Hand has a complex joint structure + joint_ordering = [ + "rootJoint", + "robot0_forearm", + "robot0_WRJ1", + "robot0_WRJ0", + "robot0_FFJ3", + "robot0_FFJ2", + "robot0_FFJ1", + "robot0_FFJ0", + "robot0_MFJ3", + "robot0_MFJ2", + "robot0_MFJ1", + "robot0_MFJ0", + "robot0_RFJ3", + "robot0_RFJ2", + "robot0_RFJ1", + "robot0_RFJ0", + "robot0_LFJ4", + "robot0_LFJ3", + "robot0_LFJ2", + "robot0_LFJ1", + "robot0_LFJ0", + "robot0_THJ4", + "robot0_THJ3", + "robot0_THJ2", + "robot0_THJ1", + "robot0_THJ0", + ] + + try: + hand.add_usd( + asset_path, + xform=wp.transform(wp.vec3(0, 0, 0.5), wp.quat_identity()), + collapse_fixed_joints=False, + enable_self_collisions=False, # Disable self-collisions for performance + hide_collision_shapes=True, + verbose=False, # Reduce output verbosity + joint_ordering=joint_ordering, # Explicit joint ordering + ) + except Exception as e: + print(f"Warning: Failed to load with joint ordering: {e}") + print("Trying without joint ordering...") + # Try again without joint ordering + hand.add_usd( + asset_path, + xform=wp.transform(wp.vec3(0, 0, 0.5), wp.quat_identity()), + collapse_fixed_joints=False, + enable_self_collisions=False, + hide_collision_shapes=True, + verbose=False, + joint_ordering=None, + ) + + # Set up joint control modes + # The Shadow Hand has many joints, but due to tendons, + # we only need to control certain "actuated" joints + # for i in range(len(hand.joint_dof_mode)): + for i in [9]: + hand.joint_dof_mode[i] = newton.JointMode.TARGET_POSITION + hand.joint_target_ke[i] = 10 # Reduced from 100 + hand.joint_target_kd[i] = 1 # Reduced from 5 + + # Build the full scene + builder = newton.ModelBuilder() + builder.replicate(hand, self.num_envs, spacing=(0.8, 0.8, 0)) + builder.add_ground_plane() # Add ground to prevent falling + + self.model = builder.finalize() + + ## Apply mass scaling if requested + # if mass_scale != 1.0: + # print(f"\nApplying mass scale factor: {mass_scale}") + # # Scale all body masses (which means scaling inverse masses down) + # self.model.body_inv_mass = self.model.body_inv_mass / mass_scale + # # Also scale inertias + # self.model.body_inv_inertia = self.model.body_inv_inertia / mass_scale + + # Print tendon info + if self.model.tendon_count > 0: + print(f"\nModel has {self.model.tendon_count} tendons:") + # Convert arrays to numpy for indexing + tendon_start = ( + self.model.tendon_start.numpy() + if hasattr(self.model.tendon_start, "numpy") + else self.model.tendon_start + ) + tendon_key = self.model.tendon_key if hasattr(self.model, "tendon_key") else [] + + tendon_params = ( + self.model.tendon_params.numpy() + if hasattr(self.model.tendon_params, "numpy") + else self.model.tendon_params + ) + # tendon_joints = self.model.tendon_joints.numpy() if hasattr(self.model.tendon_joints, 'numpy') else self.model.tendon_joints + # tendon_gearings = self.model.tendon_gearings.numpy() if hasattr(self.model.tendon_gearings, 'numpy') else self.model.tendon_gearings + + for i in range(self.model.tendon_count): + tendon_name = tendon_key[i] if i < len(tendon_key) else f"tendon_{i}" + start_idx = tendon_start[i] + end_idx = tendon_start[i + 1] if i + 1 < len(tendon_start) else len(self.model.tendon_joints) + num_joints = end_idx - start_idx + params = tendon_params[i] + print(f" Tendon '{tendon_name}': couples {num_joints} joints") + print(f" Parameters: ke={params[0]}, kd={params[1]}, rest_length={params[2]}") + + # Check for extreme values + if params[0] > 1e4: + print(f" WARNING: Very high stiffness ke={params[0]}") + if params[2] < 0: + print(f" WARNING: Negative rest length={params[2]}") + + # Optionally disable tendons by zeroing their stiffness + if disable_tendons: + print("\nDisabling tendons for debugging...") + # Convert to numpy, modify, then copy back + tendon_params_np = self.model.tendon_params.numpy() + for i in range(self.model.tendon_count): + tendon_params_np[i][0] = 0.0 # Set ke to 0 + self.model.tendon_params.assign(tendon_params_np) + + # Create solver + try: + self.solver = newton.solvers.SolverMuJoCo( + self.model, + iterations=50, + ls_iterations=25, + use_mujoco_cpu=False, + solver="newton", + integrator="implicit", # Use implicit integrator for better stability + separate_envs_to_worlds=False, # Avoid environment separation issues + nefc_per_env=500, # Increase contact constraint buffer size for Shadow Hand + ) + except Exception as e: + print(f"Error creating MuJoCo solver: {e}") + print("Trying with CPU MuJoCo...") + self.solver = newton.solvers.SolverMuJoCo( + self.model, + iterations=50, + ls_iterations=25, + use_mujoco_cpu=True, # Fall back to CPU + solver="newton", + integrator="implicit", # Use implicit integrator for better stability + separate_envs_to_worlds=False, + nefc_per_env=500, # Increase contact constraint buffer size for Shadow Hand + ) + + self.state_0 = self.model.state() + self.state_1 = self.model.state() + self.control = self.model.control() + + # Debug: Check for bodies with zero or very small mass + print("\nChecking body masses and inertias:") + body_masses = ( + self.model.body_inv_mass.numpy() if hasattr(self.model.body_inv_mass, "numpy") else self.model.body_inv_mass + ) + body_inertias = ( + self.model.body_inv_inertia.numpy() + if hasattr(self.model.body_inv_inertia, "numpy") + else self.model.body_inv_inertia + ) + + min_mass = float("inf") + max_mass = 0 + mass_issues = [] + + for i in range(self.model.body_count): + if i < len(self.model.body_key): + body_name = self.model.body_key[i] + else: + body_name = f"body_{i}" + + if body_masses[i] == 0: + mass = "infinite (fixed)" + else: + mass = 1.0 / body_masses[i] + if mass < min_mass and mass > 0: + min_mass = mass + if mass > max_mass: + max_mass = mass + + if mass < 1e-4: + mass_issues.append(f" WARNING: Body {i} '{body_name}' has very small mass: {mass:.6f} kg") + + # Check inertia + inv_I = body_inertias[i] + if inv_I[0, 0] > 0: # Not fixed + I_xx = 1.0 / inv_I[0, 0] + if I_xx < 1e-6: + mass_issues.append(f" WARNING: Body {i} '{body_name}' has very small inertia: {I_xx:.6e}") + + if mass_issues: + print("\nMass/Inertia warnings:") + for issue in mass_issues[:10]: # Show first 10 + print(issue) + if len(mass_issues) > 10: + print(f" ... and {len(mass_issues) - 10} more warnings") + + if max_mass > 0 and min_mass < float("inf"): + mass_ratio = max_mass / min_mass + print(f"\nMass ratio (max/min): {mass_ratio:.1f}") + if mass_ratio > 1000: + print(" WARNING: Very large mass ratio can cause instability!") + + self.contacts = self.model.collide(self.state_0) + + # Store initial joint positions + self.initial_joint_q = self.state_0.joint_q.numpy().copy() + + # Identify key finger joints for control + # We'll animate some finger flexion/extension + self.finger_joints = [] + joint_names = [self.model.joint_key[i] for i in range(self.model.joint_dof_count)] + + # Look for finger flexion joints (these names are typical for Shadow Hand) + finger_prefixes = ["FFJ", "MFJ", "RFJ", "LFJ", "THJ"] # First, Middle, Ring, Little, Thumb + for i, name in enumerate(joint_names): + for prefix in finger_prefixes: + if prefix in name and "J2" in name: # J2 is typically the main flexion joint + self.finger_joints.append(i) + print(f"Found finger joint: {name} at index {i}") + break + + self.viewer.set_model(self.model) + self.capture() + + # Print summary + print("\n" + "=" * 50) + print("Shadow Hand Example Configuration:") + print(f" Mass scale factor: {mass_scale}") + print(f" Stiffness scale factor: {stiffness_scale}") + print(f" Simulation timestep: {self.sim_dt:.6f} s") + print(f" Substeps per frame: {self.sim_substeps}") + print(f" Tendons: {'DISABLED' if disable_tendons else 'ENABLED'}") + print(" Integrator: implicit") + print("=" * 50 + "\n") + + def capture(self): + self.graph = None + if wp.get_device().is_cuda: + with wp.ScopedCapture() as capture: + self.simulate() + self.graph = capture.graph + + def simulate(self): + self.contacts = self.model.collide(self.state_0) + for _ in range(self.sim_substeps): + self.state_0.clear_forces() + + # Apply sinusoidal motion to finger joints + # This will demonstrate how tendons couple the motion + phase = self.sim_time * 2.0 # 2 rad/s frequency + + # Create target positions + target_q = self.initial_joint_q.copy() + + # Animate finger joints with different phases + for i, joint_idx in enumerate(self.finger_joints): + # Each finger moves with a slight phase offset + finger_phase = phase + i * 0.5 + # Oscillate between 0 and 60 degrees + angle = 0.3 * (1.0 + math.sin(finger_phase)) + target_q[joint_idx] = angle + + # Set target positions + self.control.joint_target.assign(target_q) + + # Apply viewer forces (for interaction) + self.viewer.apply_forces(self.state_0) + + self.solver.step(self.state_0, self.state_1, self.control, self.contacts, self.sim_dt) + + # Swap states + self.state_0, self.state_1 = self.state_1, self.state_0 + + 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(self): + # Run a few steps to verify tendons are working + for _ in range(10): + self.step() + + # Check that joints have moved + current_q = self.state_0.joint_q.numpy() + q_diff = current_q - self.initial_joint_q + max_diff = abs(q_diff).max() + + print(f"Test: Maximum joint position change: {max_diff:.4f} rad") + assert max_diff > 0.01, "Joints should have moved" + print("Test passed: Tendons are coupling joint motion correctly") + + +if __name__ == "__main__": + parser = newton.examples.create_parser() + parser.add_argument("--num-envs", type=int, default=4, help="Total number of simulated environments.") + parser.add_argument("--disable-tendons", action="store_true", help="Disable tendons for debugging") + parser.add_argument( + "--mass-scale", type=float, default=1.0, help="Scale all masses by this factor (try 10 or 100 for stability)" + ) + + viewer, args = newton.examples.init(parser) + + example = Example(viewer, args.num_envs, args.disable_tendons, args.mass_scale) + + newton.examples.run(example) diff --git a/newton/examples/robot/example_robot_simple_tendon.py b/newton/examples/robot/example_robot_simple_tendon.py new file mode 100644 index 000000000..f309ecc76 --- /dev/null +++ b/newton/examples/robot/example_robot_simple_tendon.py @@ -0,0 +1,327 @@ +# 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 Simple Tendon Robot +# +# Shows a simple demonstration of fixed tendons coupling joint motion. +# Creates a 3-link finger where: +# - Joint 1 (base) is actively controlled with position control +# - Joint 2 is passive and coupled to joint 1 via a tendon +# - Joint 3 is passive (free to move based on dynamics) +# +# The example demonstrates how tendons can create mechanical coupling +# between joints, similar to how finger tendons work in robotic hands. +# +# What you should see: +# - The base joint oscillates back and forth +# - Joint 2 follows joint 1's motion (80% same direction) +# - Joint 3 moves freely based on gravity and dynamics +# - Creates a natural finger curling motion +# +# Command: python -m newton.examples robot_simple_tendon --num-envs 4 +# +########################################################################### + +import math + +import numpy as np +import warp as wp + +import newton +import newton.examples + + +class Example: + def __init__(self, viewer, num_envs=4): + self.fps = 60 + self.frame_dt = 1.0 / self.fps + self.sim_time = 0.0 + self.sim_substeps = 4 + self.sim_dt = self.frame_dt / self.sim_substeps + + self.num_envs = num_envs + self.viewer = viewer + + # Create a simple 3-link finger with tendon coupling + finger = newton.ModelBuilder() + + # Configure joint and shape parameters + finger.default_joint_cfg = newton.ModelBuilder.JointDofConfig( + limit_ke=100.0, + limit_kd=100.0, + friction=0.2, + armature=0.1, # Increased armature for stability + ) + + finger.default_shape_cfg = newton.ModelBuilder.ShapeConfig( + mu=0.5, + ke=1.0e3, + kd=1.0e2, + kf=1.0e2, + density=1000.0, # kg/m^3 - density for future automatic mass/inertia computation + ) + + # Create 3 links + link_length = 0.15 + link_radius = 0.02 + link_mass = 0.1 + + # Link 1 + link1 = finger.add_body(mass=link_mass, key="link1") + + # Link 2 + link2 = finger.add_body(mass=link_mass, key="link2") + + # Link 3 + link3 = finger.add_body(mass=link_mass, key="link3") + + # Create joints + # Joint 1: world to link1 (this will be actuated) + joint1 = finger.add_joint_revolute( + parent=-1, # Attach to world + child=link1, + parent_xform=wp.transform(wp.vec3(0.0, 0.0, link_length), wp.quat_identity()), + child_xform=wp.transform(wp.vec3(0.0, 0.0, -link_length / 2), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), # Rotate around X + limit_lower=-math.pi / 2, + limit_upper=math.pi / 2, + key="joint1", + ) + + # Joint 2: link1 to link2 (coupled by tendon) + joint2 = finger.add_joint_revolute( + parent=link1, + child=link2, + parent_xform=wp.transform(wp.vec3(0.0, 0.0, link_length / 2), wp.quat_identity()), + child_xform=wp.transform(wp.vec3(0.0, 0.0, -link_length / 2), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), + limit_lower=-math.pi / 2, # Reasonable limits + limit_upper=math.pi / 2, + key="joint2", + ) + + # Joint 3: link2 to link3 (coupled by tendon) + joint3 = finger.add_joint_revolute( + parent=link2, + child=link3, + parent_xform=wp.transform(wp.vec3(0.0, 0.0, link_length / 2), wp.quat_identity()), + child_xform=wp.transform(wp.vec3(0.0, 0.0, -link_length / 2), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), + limit_lower=-math.pi / 2, # Reasonable limits + limit_upper=math.pi / 2, + key="joint3", + ) + + # Set joint control modes + finger.joint_dof_mode[joint1] = newton.JointMode.TARGET_POSITION + finger.joint_target_ke[joint1] = 50.0 + finger.joint_target_kd[joint1] = 5.0 + + # Joint 2 is passive (no position control) + finger.joint_dof_mode[joint2] = newton.JointMode.NONE + + # Joint 3 is passive (controlled by tendon) + finger.joint_dof_mode[joint3] = newton.JointMode.NONE + + # Add a tendon that couples joint1 to joint2 + # When joint1 moves, joint2 follows with some ratio + finger.add_tendon( + name="finger_tendon1", + joint_ids=[joint1, joint2], + gearings=[1.0, -0.8], # Joint2 moves 80% with joint1 + stiffness=100.0, # Moderate stiffness + damping=10.0, # Moderate damping + rest_length=0.0, + ) + + # Add a tendon that couples joint1 to joint3 + # When joint1 moves, joint3 follows with some ratio + finger.add_tendon( + name="finger_tendon2", + joint_ids=[joint1, joint3], + gearings=[1.0, -0.6], # Joint3 moves 80% with joint2 + stiffness=100.0, # Moderate stiffness + damping=10.0, # Moderate damping + rest_length=0.0, + ) + + # Add visual shapes + # Position capsules to align with the center of mass + finger.add_shape_capsule( + body=link1, + xform=wp.transform(wp.vec3(0.0, 0.0, 0), wp.quat_identity()), + radius=link_radius, + half_height=link_length / 2, + ) + + finger.add_shape_capsule( + body=link2, + xform=wp.transform(wp.vec3(0.0, 0.0, 0), wp.quat_identity()), + radius=link_radius, + half_height=link_length / 2, + ) + + finger.add_shape_capsule( + body=link3, + xform=wp.transform(wp.vec3(0.0, 0.0, 0), wp.quat_identity()), + radius=link_radius, + half_height=link_length / 2, + ) + + # Build full scene + builder = newton.ModelBuilder() + builder.replicate(finger, self.num_envs, spacing=(0.6, 0.6, 0)) + builder.add_ground_plane() + + self.model = builder.finalize() + + # Print tendon info + if hasattr(self.model, "tendon_count") and self.model.tendon_count > 0: + print(f"\nModel has {self.model.tendon_count} tendons:") + # Convert warp arrays to numpy for indexing + tendon_start_np = self.model.tendon_start.numpy() + tendon_params_np = self.model.tendon_params.numpy() + + for i in range(self.model.tendon_count): + tendon_name = self.model.tendon_key[i] if i < len(self.model.tendon_key) else f"tendon_{i}" + start_idx = tendon_start_np[i] + end_idx = tendon_start_np[i + 1] if i + 1 < len(tendon_start_np) else len(self.model.tendon_joints) + num_joints = end_idx - start_idx + params = tendon_params_np[i] + print(f" Tendon '{tendon_name}': couples {num_joints} joints") + print(f" Stiffness: {params[0]}, Damping: {params[1]}") + + # Create solver + self.solver = newton.solvers.SolverMuJoCo( + self.model, + iterations=20, + ls_iterations=10, + ) + + 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) + + # Store initial joint positions for reference + self.initial_joint_q = self.state_0.joint_q.numpy().copy() + + # We'll control only the first joint of each finger + self.actuated_joints = list(range(0, self.model.joint_dof_count, 3)) + print(f"\nActuating joints: {self.actuated_joints}") + + # Show which joints are passive and coupled by tendons + passive_joints = [] + for i in range(self.model.joint_dof_count): + if i not in self.actuated_joints: + passive_joints.append(i) + print(f"Passive joints (driven by tendon): {passive_joints}") + + self.viewer.set_model(self.model) + self.capture() + + def capture(self): + self.graph = None + if wp.get_device().is_cuda: + with wp.ScopedCapture() as capture: + self.simulate() + self.graph = capture.graph + + def simulate(self): + self.contacts = self.model.collide(self.state_0) + for _ in range(self.sim_substeps): + self.state_0.clear_forces() + + # Apply viewer forces (for interaction) + self.viewer.apply_forces(self.state_0) + + self.solver.step(self.state_0, self.state_1, self.control, self.contacts, self.sim_dt) + + # Swap states + self.state_0, self.state_1 = self.state_1, self.state_0 + + def step(self): + # Apply sinusoidal motion to first joint only + # The tendon will couple the motion to the other joints + phase = self.sim_time * 1.0 # 2 rad/s frequency + + # Create target positions array + target_q = np.zeros(self.model.joint_dof_count) + + # Animate only the first joint of each finger + for _i, joint_idx in enumerate(self.actuated_joints): + # Oscillate between -45 and +45 degrees + angle = 0.6 * math.sin(phase) + target_q[joint_idx] = angle + + # Set target positions in control + target_q_wp = wp.array(target_q, dtype=float, device=self.model.device) + wp.copy(self.control.joint_target, target_q_wp) + + if self.graph: + wp.capture_launch(self.graph) + else: + self.simulate() + + self.sim_time += self.frame_dt + + # Debug: Print joint positions occasionally + if int(self.sim_time * 10) % 10 == 0: # Every second + joint_q = self.state_0.joint_q.numpy() + if len(joint_q) >= 3: + print( + f"Time {self.sim_time:.1f}s: Joint positions: " + f"q1={joint_q[0]:.3f}, q2={joint_q[1]:.3f}, q3={joint_q[2]:.3f}, " + f"constraint={joint_q[0] + 0.8 * joint_q[1]:.3f}" + ) + + 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(self): + # Run a few steps to verify tendons are working + for _ in range(10): + self.step() + + # Check that coupled joints have moved together + current_q = self.state_0.joint_q.numpy() + + # For each finger, check that joint2 and joint3 maintain the coupling + for finger in range(self.num_envs): + base_idx = finger * 3 + q1 = current_q[base_idx] # First joint (actuated) + q2 = current_q[base_idx + 1] # Second joint (tendon coupled) + q3 = current_q[base_idx + 2] # Third joint (tendon coupled) + + # Verify that joint2 and joint3 maintain approximate ratio + # The gearing is [1.0, 0.8], so we expect q3 ≈ 0.8 * q2 + if abs(q2) > 0.01: # Only check if joint2 has moved + ratio = q3 / q2 + print(f"Finger {finger}: q1={q1:.3f}, q2={q2:.3f}, q3={q3:.3f}, ratio={ratio:.3f}") + + +if __name__ == "__main__": + parser = newton.examples.create_parser() + parser.add_argument("--num-envs", type=int, default=4, help="Total number of simulated environments.") + + viewer, args = newton.examples.init(parser) + + example = Example(viewer, args.num_envs) + + newton.examples.run(example) diff --git a/newton/tests/assets/tendons_test.usda b/newton/tests/assets/tendons_test.usda new file mode 100644 index 000000000..303ad1567 --- /dev/null +++ b/newton/tests/assets/tendons_test.usda @@ -0,0 +1,144 @@ +#usda 1.0 +( + upAxis = "Z" + metersPerUnit = 1.0 +) + +def Xform "World" +{ + def Xform "Articulation" ( + prepend apiSchemas = ["PhysicsArticulationRootAPI"] + ) + { + def Xform "Base" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + def Cube "Cube" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + double size = 1.0 + } + + float physics:mass = 0.0 + } + + def Xform "Link1" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + double3 xformOp:translate = (0, 0, 1) + uniform token[] xformOpOrder = ["xformOp:translate"] + + def Cube "Cube" + { + float3[] extent = [(-0.25, -0.25, -0.5), (0.25, 0.25, 0.5)] + } + + float physics:mass = 1.0 + float3 physics:diagonalInertia = (0.1, 0.1, 0.01) + + def PhysicsRevoluteJoint "Joint1" ( + prepend apiSchemas = ["PhysxTendonAxisRootAPI:tendon1", "PhysxTendonAxisAPI:tendon2"] + ) + { + uniform token physics:axis = "X" + rel physics:body0 = + rel physics:body1 = + float physics:lowerLimit = -90 + float physics:upperLimit = 90 + point3f physics:localPos0 = (0, 0, 0.5) + point3f physics:localPos1 = (0, 0, -0.5) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + + # Tendon 1 properties (this is the root) + float[] physxTendon:tendon1:gearing = [1.0] + float physxTendon:tendon1:stiffness = 100.0 + float physxTendon:tendon1:damping = 10.0 + float physxTendon:tendon1:restLength = 0.0 + float physxTendon:tendon1:lowerLimit = -0.5 + float physxTendon:tendon1:upperLimit = 0.5 + + # Tendon 2 properties (this is a participant) + float[] physxTendon:tendon2:gearing = [2.0] + } + } + + def Xform "Link2" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + double3 xformOp:translate = (0, 0, 2) + uniform token[] xformOpOrder = ["xformOp:translate"] + + def Cube "Cube" + { + float3[] extent = [(-0.25, -0.25, -0.5), (0.25, 0.25, 0.5)] + } + + float physics:mass = 1.0 + float3 physics:diagonalInertia = (0.1, 0.1, 0.01) + + def PhysicsRevoluteJoint "Joint2" ( + prepend apiSchemas = ["PhysxTendonAxisAPI:tendon1", "PhysxTendonAxisRootAPI:tendon2"] + ) + { + uniform token physics:axis = "X" + rel physics:body0 = + rel physics:body1 = + float physics:lowerLimit = -90 + float physics:upperLimit = 90 + point3f physics:localPos0 = (0, 0, 0.5) + point3f physics:localPos1 = (0, 0, -0.5) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + + # Tendon 1 properties (this is a participant) + float[] physxTendon:tendon1:gearing = [-1.0] + + # Tendon 2 properties (this is the root) + float[] physxTendon:tendon2:gearing = [-2.0] + float physxTendon:tendon2:stiffness = 50.0 + float physxTendon:tendon2:damping = 5.0 + float physxTendon:tendon2:restLength = 0.1 + float physxTendon:tendon2:lowerLimit = -1.0 + float physxTendon:tendon2:upperLimit = 1.0 + } + } + + def Xform "Link3" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + double3 xformOp:translate = (0, 0, 3) + uniform token[] xformOpOrder = ["xformOp:translate"] + + def Cube "Cube" + { + float3[] extent = [(-0.25, -0.25, -0.5), (0.25, 0.25, 0.5)] + } + + float physics:mass = 1.0 + float3 physics:diagonalInertia = (0.1, 0.1, 0.01) + + def PhysicsRevoluteJoint "Joint3" ( + prepend apiSchemas = ["PhysxTendonAxisAPI:tendon2"] + ) + { + uniform token physics:axis = "X" + rel physics:body0 = + rel physics:body1 = + float physics:lowerLimit = -90 + float physics:upperLimit = 90 + point3f physics:localPos0 = (0, 0, 0.5) + point3f physics:localPos1 = (0, 0, -0.5) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + + # Tendon 2 properties (this is a participant) + float[] physxTendon:tendon2:gearing = [1.0] + } + } + } +} diff --git a/newton/tests/test_fixed_tendons.py b/newton/tests/test_fixed_tendons.py new file mode 100644 index 000000000..56395f5fc --- /dev/null +++ b/newton/tests/test_fixed_tendons.py @@ -0,0 +1,172 @@ +# 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. + +"""Tests for fixed tendon support in Newton.""" + +import unittest + +import numpy as np +import warp as wp + +import newton + + +class TestFixedTendons(unittest.TestCase): + """Test cases for fixed tendon functionality.""" + + def test_add_tendon_basic(self): + """Test basic tendon creation and validation.""" + builder = newton.ModelBuilder() + + # Create simple chain + ground = builder.add_body(mass=0.0) + bodies = [] + joints = [] + + for _i in range(3): + body = builder.add_body(mass=1.0) + joint = builder.add_joint_revolute( + parent=bodies[-1] if bodies else ground, + child=body, + parent_xform=wp.transform([0.0, 0.0, 1.0], wp.quat_identity()), + child_xform=wp.transform_identity(), + axis=[1.0, 0.0, 0.0], + ) + bodies.append(body) + joints.append(joint) + + # Add tendon + tendon_id = builder.add_tendon( + name="test_tendon", + joint_ids=[joints[0], joints[2]], + gearings=[1.0, -1.0], + stiffness=100.0, + damping=10.0, + rest_length=0.0, + lower_limit=-1.0, + upper_limit=1.0, + ) + + self.assertEqual(tendon_id, 0) + self.assertEqual(len(builder.tendon_start), 1) + self.assertEqual(len(builder.tendon_key), 1) + self.assertEqual(builder.tendon_key[0], "test_tendon") + + def test_add_tendon_validation(self): + """Test tendon creation validation.""" + builder = newton.ModelBuilder() + + # Create minimal setup + body1 = builder.add_body(mass=1.0) + body2 = builder.add_body(mass=1.0) + joint1 = builder.add_joint_revolute( + parent=-1, + child=body1, + parent_xform=wp.transform_identity(), + child_xform=wp.transform_identity(), + axis=[1.0, 0.0, 0.0], + ) + joint2 = builder.add_joint_revolute( + parent=body1, + child=body2, + parent_xform=wp.transform_identity(), + child_xform=wp.transform_identity(), + axis=[1.0, 0.0, 0.0], + ) + + # Test single joint error + with self.assertRaises(ValueError): + builder.add_tendon(name="bad_tendon", joint_ids=[joint1], gearings=[1.0]) + + # Test mismatched gearings error + with self.assertRaises(ValueError): + builder.add_tendon( + name="bad_tendon", + joint_ids=[joint1, joint2], + gearings=[1.0], # Should have 2 gearings + ) + + def test_tendon_finalization(self): + """Test tendon data transfer to Model.""" + builder = newton.ModelBuilder() + + # Create joints + body = builder.add_body(mass=1.0, key="body1") + joint1 = builder.add_joint_revolute( + parent=-1, + child=body, + parent_xform=wp.transform([0.0, 0.0, 0.0], wp.quat_identity()), + child_xform=wp.transform_identity(), + axis=[1.0, 0.0, 0.0], + ) + body2 = builder.add_body(mass=1.0, key="body2") + joint2 = builder.add_joint_revolute( + parent=body, + child=body2, + parent_xform=wp.transform([0.0, 0.0, 1.0], wp.quat_identity()), + child_xform=wp.transform_identity(), + axis=[1.0, 0.0, 0.0], + ) + + # Add tendons + builder.add_tendon( + name="tendon1", + joint_ids=[joint1, joint2], + gearings=[1.0, -1.0], + stiffness=50.0, + damping=5.0, + rest_length=0.1, + ) + + builder.add_tendon( + name="tendon2", + joint_ids=[joint1, joint2], + gearings=[2.0, -2.0], + stiffness=100.0, + damping=10.0, + rest_length=0.0, + lower_limit=-0.5, + upper_limit=0.5, + ) + + # Finalize + model = builder.finalize() + + # Check counts + self.assertEqual(model.tendon_count, 2) + + # Check arrays exist + self.assertIsNotNone(model.tendon_start) + self.assertIsNotNone(model.tendon_params) + self.assertIsNotNone(model.tendon_joints) + self.assertIsNotNone(model.tendon_gearings) + self.assertEqual(len(model.tendon_key), 2) + + # Check data + tendon_start = model.tendon_start.numpy() + tendon_params = model.tendon_params.numpy() + + # First tendon + self.assertEqual(tendon_start[0], 0) + self.assertEqual(tendon_start[1], 2) + np.testing.assert_array_almost_equal(tendon_params[0], [50.0, 5.0, 0.1, float("-inf"), float("inf")], decimal=5) + + # Second tendon + self.assertEqual(tendon_start[2], 4) + np.testing.assert_array_almost_equal(tendon_params[1], [100.0, 10.0, 0.0, -0.5, 0.5], decimal=5) + + +if __name__ == "__main__": + unittest.main() diff --git a/newton/tests/test_import_usd.py b/newton/tests/test_import_usd.py index a5dde2b79..e42c6f32c 100644 --- a/newton/tests/test_import_usd.py +++ b/newton/tests/test_import_usd.py @@ -14,6 +14,7 @@ # limitations under the License. import os +import tempfile import unittest import numpy as np @@ -511,6 +512,267 @@ def test_force_limits(self): joint_dof_idx_AD = model.joint_qd_start.numpy()[joint_idx_AD] self.assertEqual(model.joint_effort_limit.numpy()[joint_dof_idx_AD], 30.0) + @unittest.skipUnless(USD_AVAILABLE, "Requires usd-core") + def test_import_tendons(self): + """Test importing USD with PhysX fixed tendons.""" + builder = newton.ModelBuilder() + + # Import the test USD with tendons + builder.add_usd(os.path.join(os.path.dirname(__file__), "assets", "tendons_test.usda"), verbose=False) + + # Check basic model structure + self.assertEqual(builder.body_count, 4) # Base + 3 links + self.assertEqual(builder.joint_count, 4) # 3 revolute joints + 1 free joint (articulation root) + + # Finalize the model + model = builder.finalize() + + # Verify tendon count + self.assertEqual(model.tendon_count, 2) + + # Get tendon data + tendon_start = model.tendon_start.numpy() + tendon_params = model.tendon_params.numpy() + tendon_joints = model.tendon_joints.numpy() + tendon_gearings = model.tendon_gearings.numpy() + + # Find joint indices by name + joint1_idx = model.joint_key.index("/World/Articulation/Link1/Joint1") + joint2_idx = model.joint_key.index("/World/Articulation/Link2/Joint2") + joint3_idx = model.joint_key.index("/World/Articulation/Link3/Joint3") + + # Test Tendon 1 (connects Joint1 and Joint2) + tendon1_idx = model.tendon_key.index("tendon1") + start1 = tendon_start[tendon1_idx] + end1 = tendon_start[tendon1_idx + 1] + + # Should have 2 joints + self.assertEqual(end1 - start1, 2) + + # Check joint attachments + tendon1_joints = sorted(tendon_joints[start1:end1].tolist()) + expected_joints1 = sorted([joint1_idx, joint2_idx]) + self.assertEqual(tendon1_joints, expected_joints1) + + # Check gearings for tendon1 + joint_gearing_map1 = {} + for i in range(start1, end1): + joint_gearing_map1[tendon_joints[i]] = tendon_gearings[i] + + self.assertAlmostEqual(joint_gearing_map1[joint1_idx], 1.0, places=5) + self.assertAlmostEqual(joint_gearing_map1[joint2_idx], -1.0, places=5) + + # Check tendon1 parameters (from root joint - Joint1) + params1 = tendon_params[tendon1_idx] + self.assertAlmostEqual(params1[0], 100.0, places=5) # stiffness + self.assertAlmostEqual(params1[1], 10.0, places=5) # damping + self.assertAlmostEqual(params1[2], 0.0, places=5) # rest_length + self.assertAlmostEqual(params1[3], -0.5, places=5) # lower_limit + self.assertAlmostEqual(params1[4], 0.5, places=5) # upper_limit + + # Test Tendon 2 (connects Joint1, Joint2, and Joint3) + tendon2_idx = model.tendon_key.index("tendon2") + start2 = tendon_start[tendon2_idx] + end2 = tendon_start[tendon2_idx + 1] + + # Should have 3 joints + self.assertEqual(end2 - start2, 3) + + # Check joint attachments + tendon2_joints = sorted(tendon_joints[start2:end2].tolist()) + expected_joints2 = sorted([joint1_idx, joint2_idx, joint3_idx]) + self.assertEqual(tendon2_joints, expected_joints2) + + # Check gearings for tendon2 + joint_gearing_map2 = {} + for i in range(start2, end2): + joint_gearing_map2[tendon_joints[i]] = tendon_gearings[i] + + self.assertAlmostEqual(joint_gearing_map2[joint1_idx], 2.0, places=5) + self.assertAlmostEqual(joint_gearing_map2[joint2_idx], -2.0, places=5) + self.assertAlmostEqual(joint_gearing_map2[joint3_idx], 1.0, places=5) + + # Check tendon2 parameters (from root joint - Joint2) + params2 = tendon_params[tendon2_idx] + self.assertAlmostEqual(params2[0], 50.0, places=5) # stiffness + self.assertAlmostEqual(params2[1], 5.0, places=5) # damping + self.assertAlmostEqual(params2[2], 0.1, places=5) # rest_length + self.assertAlmostEqual(params2[3], -1.0, places=5) # lower_limit + self.assertAlmostEqual(params2[4], 1.0, places=5) # upper_limit + + def test_import_single_joint_tendon(self): + """Test that tendons with only one joint are skipped during import.""" + # Create a USDA with a tendon that only has one joint + usda_content = """#usda 1.0 +( + upAxis = "Z" + metersPerUnit = 1.0 +) + +def Xform "World" +{ + def Xform "Articulation" ( + prepend apiSchemas = ["PhysicsArticulationRootAPI"] + ) + { + def Xform "Base" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + float physics:mass = 0.0 + } + + def Xform "Link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + double3 xformOp:translate = (0, 0, 1) + uniform token[] xformOpOrder = ["xformOp:translate"] + float physics:mass = 1.0 + float3 physics:diagonalInertia = (0.1, 0.1, 0.1) + + def PhysicsRevoluteJoint "Joint" ( + prepend apiSchemas = ["PhysxTendonAxisRootAPI:invalid_tendon"] + ) + { + uniform token physics:axis = "X" + rel physics:body0 = + rel physics:body1 = + float physics:lowerLimit = -90 + float physics:upperLimit = 90 + + # Single joint tendon - should be skipped + float[] physxTendon:invalid_tendon:gearing = [1.0] + float physxTendon:invalid_tendon:stiffness = 100.0 + } + } + } +} +""" + + # Write to temporary file and import + # Create temporary file + fd, tmp_path = tempfile.mkstemp(suffix=".usda", text=True) + try: + # Write content and close file descriptor + with os.fdopen(fd, "w") as tmp: + tmp.write(usda_content) + + # Import USD + builder = newton.ModelBuilder() + builder.add_usd(tmp_path, verbose=False) + model = builder.finalize() + + # Should have no tendons since a tendon with only one joint is invalid + self.assertEqual(model.tendon_count, 0) + finally: + try: + os.unlink(tmp_path) + except Exception: + pass # Ignore errors on cleanup + + def test_import_tendons_no_params(self): + """Test that tendons with missing parameters use defaults.""" + # Create a USDA with minimal tendon parameters + usda_content = """#usda 1.0 +( + upAxis = "Z" + metersPerUnit = 1.0 +) + +def Xform "World" +{ + def Xform "Articulation" ( + prepend apiSchemas = ["PhysicsArticulationRootAPI"] + ) + { + def Xform "Body0" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + float physics:mass = 0.0 + } + + def Xform "Body1" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + double3 xformOp:translate = (0, 0, 1) + uniform token[] xformOpOrder = ["xformOp:translate"] + float physics:mass = 1.0 + float3 physics:diagonalInertia = (0.1, 0.1, 0.1) + + def PhysicsRevoluteJoint "Joint" ( + prepend apiSchemas = ["PhysxTendonAxisRootAPI:minimal_tendon"] + ) + { + uniform token physics:axis = "X" + rel physics:body0 = + rel physics:body1 = + float physics:lowerLimit = -90 + float physics:upperLimit = 90 + + # Root joint with only gearing - no other parameters + float[] physxTendon:minimal_tendon:gearing = [1.0] + } + } + + def Xform "Body2" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + double3 xformOp:translate = (0, 0, 2) + uniform token[] xformOpOrder = ["xformOp:translate"] + float physics:mass = 1.0 + float3 physics:diagonalInertia = (0.1, 0.1, 0.1) + + def PhysicsRevoluteJoint "Joint" ( + prepend apiSchemas = ["PhysxTendonAxisAPI:minimal_tendon"] + ) + { + uniform token physics:axis = "X" + rel physics:body0 = + rel physics:body1 = + float physics:lowerLimit = -90 + float physics:upperLimit = 90 + + # Participant joint + float[] physxTendon:minimal_tendon:gearing = [-1.0] + } + } + } +} +""" + + # Write to temporary file and import + # Create temporary file + fd, tmp_path = tempfile.mkstemp(suffix=".usda", text=True) + try: + # Write content and close file descriptor + with os.fdopen(fd, "w") as tmp: + tmp.write(usda_content) + + # Import USD + builder = newton.ModelBuilder() + builder.add_usd(tmp_path, verbose=False) + model = builder.finalize() + + # Should have one tendon + self.assertEqual(model.tendon_count, 1) + + # Check that default parameters were used + params = model.tendon_params.numpy()[0] + self.assertAlmostEqual(params[0], 0.0, places=5) # default stiffness + self.assertAlmostEqual(params[1], 0.0, places=5) # default damping + self.assertAlmostEqual(params[2], 0.0, places=5) # default rest_length + self.assertTrue(np.isinf(params[3]) and params[3] < 0) # default lower_limit (-inf) + self.assertTrue(np.isinf(params[4]) and params[4] > 0) # default upper_limit (+inf) + finally: + try: + os.unlink(tmp_path) + except Exception: + pass # Ignore errors on cleanup + if __name__ == "__main__": unittest.main(verbosity=2, failfast=True) diff --git a/newton/tests/test_mujoco_solver.py b/newton/tests/test_mujoco_solver.py index c9f8acede..b5df2cfa0 100644 --- a/newton/tests/test_mujoco_solver.py +++ b/newton/tests/test_mujoco_solver.py @@ -1359,5 +1359,303 @@ def test_global_joint_solver_params(self): ) +class TestMuJoCoSolverTendons(unittest.TestCase): + def test_tendon_constraint(self): + """Test that fixed tendons correctly constrain joint motion in MuJoCo solver.""" + # Create a model with two revolute joints connected by a tendon + builder = newton.ModelBuilder() + + # Create two bodies attached to world + link1 = builder.add_body( + mass=1.0, com=wp.vec3(0.0, 0.0, 0.0), I_m=(0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1) + ) + link2 = builder.add_body( + mass=1.0, com=wp.vec3(0.0, 0.0, 0.0), I_m=(0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1) + ) + + # Add revolute joints attached to world + joint1 = builder.add_joint_revolute( + parent=-1, # World + child=link1, + parent_xform=wp.transform((0.0, 0.0, 0.0), wp.quat_identity()), + child_xform=wp.transform((0.0, 0.0, -0.5), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), # Rotate around X axis + key="joint1", + ) + + joint2 = builder.add_joint_revolute( + parent=-1, # World + child=link2, + parent_xform=wp.transform((0.0, 1.0, 0.0), wp.quat_identity()), + child_xform=wp.transform((0.0, 0.0, -0.5), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), # Rotate around X axis + key="joint2", + ) + + # Add a tendon connecting the two joints with gearing ratio 1:1 + # This means: tendon_length = rest_length + 1.0 * q1 + 1.0 * q2 + # So when q1 increases, q2 should decrease by the same amount to maintain constant length + builder.add_tendon( + name="test_tendon", + joint_ids=[joint1, joint2], + gearings=[1.0, 1.0], + stiffness=1000.0, # High stiffness for tight constraint + damping=10.0, + rest_length=0.0, + ) + + # Add visual shapes (optional, but good practice) + builder.add_shape_box(body=link1, hx=0.1, hy=0.1, hz=0.5) + builder.add_shape_box(body=link2, hx=0.1, hy=0.1, hz=0.5) + + # Set initial joint positions + builder.joint_q[0] = 0.0 # joint1 at 0 + builder.joint_q[1] = 0.0 # joint2 at 0 + + # Apply a torque only to joint1 + model = builder.finalize() + state_in = model.state() + state_out = model.state() + control = model.control() + + # Apply positive torque to joint1 only + control.joint_f.assign([20.0, 0.0]) # Strong torque on joint1, none on joint2 + + # Create MuJoCo solver + try: + solver = SolverMuJoCo(model, iterations=50, ls_iterations=50) + except ImportError as e: + self.skipTest(f"MuJoCo or deps not installed. Skipping test: {e}") + return + + # Simulate for several steps + dt = 0.002 + num_steps = 200 # Simulate for 0.4 seconds total + contacts = model.collide(state_in) + + for _ in range(num_steps): + solver.step(state_in, state_out, control, contacts, dt) + state_in, state_out = state_out, state_in + + # Get final joint positions + final_q = state_in.joint_q.numpy() + q1_final = final_q[0] + q2_final = final_q[1] + + # Verify the tendon constraint is satisfied + # With gearing [1.0, 1.0], the constraint is: q1 + q2 = constant + # Since we started at q1=0, q2=0, the constant is 0 + # So we expect: q1 + q2 = 0, meaning q2 = -q1 + constraint_error = abs(q1_final + q2_final) + + # The constraint should be satisfied within a small tolerance + self.assertLess( + constraint_error, + 0.02, # 2% tolerance for numerical stability + f"Tendon constraint not satisfied: q1={q1_final:.4f}, q2={q2_final:.4f}, " + f"expected q1+q2=0, error={constraint_error:.4f}", + ) + + # Also verify that the joints actually moved (not stuck at zero) + self.assertGreater( + abs(q1_final), 0.05, f"Joint 1 should have moved significantly under applied torque, but q1={q1_final:.4f}" + ) + + # Verify q2 moved in opposite direction + self.assertLess(q2_final, -0.04, f"Joint 2 should have moved in opposite direction, but q2={q2_final:.4f}") + + def test_tendon_with_limits(self): + """Test that tendon limits are enforced in MuJoCo solver.""" + builder = newton.ModelBuilder() + + # Create simple 2-joint system with proper articulation + # First link attached to world + link1 = builder.add_body( + mass=1.0, com=wp.vec3(0.0, 0.0, 0.0), I_m=(0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1) + ) + # Second link also attached to world + link2 = builder.add_body( + mass=1.0, com=wp.vec3(0.0, 0.0, 0.0), I_m=(0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1) + ) + + joint1 = builder.add_joint_revolute( + parent=-1, + child=link1, # Attach to world + parent_xform=wp.transform_identity(), + child_xform=wp.transform((0.0, 0.0, -0.5), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), + ) + + joint2 = builder.add_joint_revolute( + parent=-1, + child=link2, # Attach to world + parent_xform=wp.transform((0.0, 1.0, 0.0), wp.quat_identity()), + child_xform=wp.transform((0.0, 0.0, -0.5), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), + ) + + # Add tendon with limits + # Tendon length = rest_length + gearing[0]*q1 + gearing[1]*q2 + # With rest_length=0, gearing=[1,1], lower=-0.5, upper=0.5 + # This means: -0.5 <= q1 + q2 <= 0.5 + builder.add_tendon( + name="limited_tendon", + joint_ids=[joint1, joint2], + gearings=[1.0, 1.0], + stiffness=100.0, + damping=1.0, + rest_length=0.0, + lower_limit=-0.5, + upper_limit=0.5, + ) + + model = builder.finalize() + + # Create solver + try: + solver = SolverMuJoCo(model, iterations=50, ls_iterations=50) + except ImportError as e: + self.skipTest(f"MuJoCo or deps not installed. Skipping test: {e}") + return + + # Test upper limit: apply positive torques to both joints + state_in = model.state() + state_out = model.state() + control = model.control() + control.joint_f.assign([10.0, 10.0]) # Strong positive torques + + dt = 0.001 + num_steps = 200 + contacts = model.collide(state_in) + + for _ in range(num_steps): + solver.step(state_in, state_out, control, contacts, dt) + state_in, state_out = state_out, state_in + + # Check that tendon length respects upper limit + q = state_in.joint_q.numpy() + tendon_length = q[0] + q[1] # With rest_length=0 and gearing=[1,1] + + self.assertLessEqual( + tendon_length, + 0.51, # Small tolerance for numerical errors + f"Tendon upper limit violated: length={tendon_length:.4f}, limit=0.5", + ) + + # Test lower limit: apply negative torques + state_in.joint_q.assign([0.0, 0.0]) + state_in.joint_qd.assign([0.0, 0.0]) + control.joint_f.assign([-10.0, -10.0]) # Strong negative torques + + for _ in range(num_steps): + solver.step(state_in, state_out, control, contacts, dt) + state_in, state_out = state_out, state_in + + # Check that tendon length respects lower limit + q = state_in.joint_q.numpy() + tendon_length = q[0] + q[1] + + self.assertGreaterEqual( + tendon_length, + -0.51, # Small tolerance for numerical errors + f"Tendon lower limit violated: length={tendon_length:.4f}, limit=-0.5", + ) + + def test_simple_tendon_constraint(self): + """Test basic tendon functionality with minimal setup""" + builder = newton.ModelBuilder() + + # Create two simple bodies with proper inertia + body1 = builder.add_body( + mass=1.0, com=wp.vec3(0.0, 0.0, 0.0), I_m=(0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1) + ) + body2 = builder.add_body( + mass=1.0, com=wp.vec3(0.0, 0.0, 0.0), I_m=(0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1) + ) + + # Create two joints without limits + joint1 = builder.add_joint_revolute( + parent=-1, + child=body1, + parent_xform=wp.transform(wp.vec3(0.0, 0.0, 0.0), wp.quat_identity()), + child_xform=wp.transform(wp.vec3(0.0, 0.0, 0.0), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), + armature=0.01, + ) + + joint2 = builder.add_joint_revolute( + parent=-1, + child=body2, + parent_xform=wp.transform(wp.vec3(0.0, 2.0, 0.0), wp.quat_identity()), + child_xform=wp.transform(wp.vec3(0.0, 0.0, 0.0), wp.quat_identity()), + axis=(1.0, 0.0, 0.0), + armature=0.01, + ) + + # Add simple tendon with 1:1 coupling + builder.add_tendon( + name="test_tendon", + joint_ids=[joint1, joint2], + gearings=[1.0, 1.0], + stiffness=1000.0, # High stiffness for tight coupling + damping=10.0, + rest_length=0.0, + ) + + model = builder.finalize() + solver = newton.solvers.SolverMuJoCo(model) + + state = model.state() + control = model.control() + + # Apply torque to joint1 + control.joint_f.assign([10.0, 0.0]) + + # Simulate for a short time + dt = 0.001 + for i in range(100): + contacts = model.collide(state) + state_new = model.state() + solver.step(state, state_new, control, contacts, dt) + state = state_new + + if i % 20 == 0: + q = state.joint_q.numpy() + print(f"Step {i}: q1={q[0]:.4f}, q2={q[1]:.4f}, sum={q[0] + q[1]:.4f}") + + # Final check + q = state.joint_q.numpy() + q1, q2 = q[0], q[1] + + # With gearing [1.0, 1.0], the constraint is q1 + q2 = constant (0) + constraint_value = q1 + q2 + + print(f"\nFinal: q1={q1:.4f}, q2={q2:.4f}, constraint={constraint_value:.4f}") + + # Joint1 should move positive under torque + self.assertGreater(q1, 0.01, "Joint1 should move under applied torque") + + # Joint2 should move negative to maintain constraint + self.assertLess(q2, -0.01, "Joint2 should move oppositely due to tendon") + + # Constraint should be satisfied within reasonable tolerance + # With finite stiffness, there will be some compliance + self.assertAlmostEqual( + constraint_value, + 0.0, + delta=0.01, # Allow 0.01 rad error (~0.6 degrees) + msg=f"Tendon constraint violated: {constraint_value:.4f} != 0", + ) + + # Verify the coupling is working correctly + # Joint positions should be approximately equal and opposite + self.assertAlmostEqual( + q1, + -q2, + delta=0.01, # Same tolerance as constraint + msg=f"Joints not moving equally and oppositely: q1={q1:.4f}, q2={q2:.4f}", + ) + + if __name__ == "__main__": unittest.main()