[Question] body_incoming_joint_wrench_b

[ChipsICU]

I'm using a single-joint-link environment (revolute_articulation) to help me understand how articulation works.

Currently, I've set zero gravity, zero friction, zero stiffness, and zero damping, and am trying to observe how the various terms in body_incoming_joint_wrench_b change over time.

Strangely, I've discovered:

1. Even with zero gravity, the Fz term in body_incoming_joint_wrench_b has a relatively large value (-9.999504) at the beginning of the environment step, seemingly close to its own weight. This value decreases as the armature increases. (1s duration)

2. The Ty term in body_incoming_joint_wrench_b represents the torque transferred from the joint term, but it increases with each step of the environment, showing a strong correlation with the angular velocity. (10s duration)

These points don't seem to align with my physics common sense. Could you please explain?

import csv
import os
import time
from datetime import datetime
import matplotlib.pyplot as plt

import torch
import numpy as np

from isaaclab.app import AppLauncher

parser = argparse.ArgumentParser(description="Armature")
parser.add_argument("--num_envs", type=int, default=1, help="number of envs")
parser.add_argument("--actuator_type", type=str, default="ideal", choices=["ideal", "implicit"], 
                    help="ideal (IdealPDActuatorCfg) or implicit (ImplicitActuatorCfg)")
AppLauncher.add_app_launcher_args(parser)
args_cli = parser.parse_args()

app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app

import isaaclab.sim as sim_utils
from isaaclab.assets import Articulation, ArticulationCfg, AssetBaseCfg
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
from isaaclab.sim import SimulationContext
from isaaclab.utils import configclass
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
from isaaclab.actuators import (
    ImplicitActuatorCfg,
    IdealPDActuatorCfg
)


@configclass
class ArmatureTestSceneCfg(InteractiveSceneCfg):
    env_spacing: float = 3.0 

    ground = AssetBaseCfg(
        prim_path="/World/defaultGroundPlane", 
        spawn=sim_utils.GroundPlaneCfg()
    )

    dome_light = AssetBaseCfg(
        prim_path="/World/Light", 
        spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
    )

    test_joint: ArticulationCfg = ArticulationCfg(
        spawn=sim_utils.UsdFileCfg(
            usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd",
            rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=True,
                max_linear_velocity=1000.0,
                max_angular_velocity=1000.0,
                max_depenetration_velocity=100.0,
                enable_gyroscopic_forces=False,
            ),
            articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=False,
                solver_position_iteration_count=4,
                solver_velocity_iteration_count=0,
                sleep_threshold=0.005,
                stabilization_threshold=0.001,
            ),
        ),
        init_state=ArticulationCfg.InitialStateCfg(
            pos=(0.0, 0.0, 3.0),
            joint_pos={".*": 0.0},
            joint_vel={".*": 0.0},
        ),
        actuators={
            "test_actuator": IdealPDActuatorCfg(
                joint_names_expr=[".*"],
                effort_limit_sim=1000.0,
                stiffness=0.0,
                damping=0.0,
                armature=1.0,
            ), 
        },
        prim_path="{ENV_REGEX_NS}/TestJoint"
    )

class ArmatureIdentificationTest:

    def __init__(self, sim: SimulationContext, scene: InteractiveScene, actuator_type: str):
        self.sim = sim
        self.scene = scene
        self.robot = scene["test_joint"]
        self.actuator_type = actuator_type
        
        self.test_duration = 10.0 
        self.sim_dt = 0.01
        self.steps_per_test = int(self.test_duration / self.sim_dt)
        self.constant_effort = 10.0 
        
        self.test_results = []
        self.detailed_data = []
        
        self.output_dir = "/home/robot/IsaacLab/motor_test/results"
        os.makedirs(self.output_dir, exist_ok=True)
        
        print(f"[INFO] Testing Actuator Type: {self.actuator_type}")
        print(f"[INFO] Test Parameters:")
        print(f"  - Duration: {self.test_duration}s")
        print(f"  - Time step: {self.sim_dt}s")
        print(f"  - Constant effort: {self.constant_effort} Nm")
    
    def verify_gravity_disabled(self):
        print("\n[INFO] Verifying gravity settings...")
        
        self._reset_robot()
        
        zero_efforts = torch.zeros_like(self.robot.data.joint_pos)
        
        initial_pos = self.robot.data.joint_pos[0, 0].item()
        initial_vel = self.robot.data.joint_vel[0, 0].item()
        
        print(f"    Initial state: pos={initial_pos:.6f}, vel={initial_vel:.6f}")
        
        for step in range(100):
            self.robot.set_joint_effort_target(zero_efforts)
            self.scene.write_data_to_sim()
            self.sim.step()
            self.scene.update(self.sim_dt)
        
        final_pos = self.robot.data.joint_pos[0, 0].item()
        final_vel = self.robot.data.joint_vel[0, 0].item()
        final_acc = self.robot.data.joint_acc[0, 0].item()
        
        print(f"    Final state: pos={final_pos:.6f}, vel={final_vel:.6f}, acc={final_acc:.6f}")
        
        pos_drift = abs(final_pos - initial_pos)
        vel_drift = abs(final_vel - initial_vel)
        
        if pos_drift > 1e-4 or vel_drift > 1e-4:
            print(f"    WARNING: Possible gravity/external force detected!")
            print(f"    Position drift: {pos_drift:.6f}")
            print(f"    Velocity drift: {vel_drift:.6f}")
            return False
        else:
            print(f"    SUCCESS: No gravity influence detected")
            return True
    
    def run_single_armature_test(self, armature_value: float):
        print(f"\n[INFO] Testing Armature = {armature_value}")
        
        self._reset_robot()
        
        self.robot.write_joint_armature_to_sim(armature_value)
        
        initial_vel = self.robot.data.joint_vel[0, 0].item()
        initial_pos = self.robot.data.joint_pos[0, 0].item()
        if abs(initial_vel) > 1e-6 or abs(initial_pos) > 1e-6:
            print(f"    Warning: Non-zero initial state detected! pos={initial_pos:.6f}, vel={initial_vel:.6f}")
            raise RuntimeError("Initial state not zero after reset!")
        
        test_data = []
        
        for step in range(self.steps_per_test):
            efforts = torch.full_like(self.robot.data.joint_pos, self.constant_effort)
            self.robot.set_joint_effort_target(efforts)
            
            self.scene.write_data_to_sim()
            
            self.sim.step()
            
            self.scene.update(self.sim_dt)
            
            current_time = step * self.sim_dt
            
            joint_effort = self.robot.data.applied_torque[0, 0].item()
            joint_vel = self.robot.data.joint_vel[0, 0].item()
            joint_acc = self.robot.data.joint_acc[0, 0].item()
            joint_pos = self.robot.data.joint_pos[0, 0].item()
            
            net_wrench = self.robot.data.body_incoming_joint_wrench_b[0, 1, :]
            reaction_wrench_norm = np.linalg.norm(net_wrench[-3:].cpu().numpy())
            joint_vel = self.robot.data.joint_vel[0, 0].item()
            joint_acc = self.robot.data.joint_acc[0, 0].item()
            joint_pos = self.robot.data.joint_pos[0, 0].item()
            
            theoretical_acc = self.constant_effort / (2.0 + armature_value)  # Base armature is 2.0
            
            reaction_force = net_wrench[:3].cpu().numpy()  
            reaction_torque = net_wrench[3:].cpu().numpy()  

            net_wrench = self.robot.data.body_incoming_joint_wrench_b[0, 1, :]
            step_data = {
                'armature': armature_value,
                'time': current_time,
                'step': step,
                'joint_effort': joint_effort,  
                'reaction_wrench_norm': reaction_wrench_norm,  
                'reaction_force_x': reaction_force[0], 
                'reaction_force_y': reaction_force[1], 
                'reaction_force_z': reaction_force[2],  
                'reaction_torque_x': reaction_torque[0],  
                'reaction_torque_y': reaction_torque[1],  
                'reaction_torque_z': reaction_torque[2],  
                
                'joint_vel': joint_vel,
                'joint_acc': joint_acc,
                # 'joint_pos': joint_pos,
                # 'applied_effort': self.constant_effort,
                'theoretical_acc': theoretical_acc
            }
            
            test_data.append(step_data)
            self.detailed_data.append(step_data)
        
        stable_start_idx = int(len(test_data) * 0.8)
        stable_data = test_data[stable_start_idx:]
        
        avg_effort = np.mean([d['joint_effort'] for d in stable_data])
        avg_vel = np.mean([d['joint_vel'] for d in stable_data])
        avg_acc = np.mean([d['joint_acc'] for d in stable_data])
        avg_theoretical_acc = np.mean([d['theoretical_acc'] for d in stable_data])
        
        result = {
            'actuator_type': self.actuator_type,
            'armature': armature_value,
            'avg_effort': avg_effort,
            'avg_vel': avg_vel,
            'avg_acc': avg_acc,
            'avg_theoretical_acc': avg_theoretical_acc,
            'applied_effort': self.constant_effort
        }
        
        self.test_results.append(result)
        
        print(f"  Result: τ={avg_effort:.6f}, ω={avg_vel:.6f}, α={avg_acc:.6f}")
        
        return result

    def _reset_robot(self):
        zero_efforts = torch.zeros_like(self.robot.data.joint_pos)
        self.robot.set_joint_effort_target(zero_efforts)
        
        joint_pos = torch.zeros_like(self.robot.data.joint_pos)
        joint_vel = torch.zeros_like(self.robot.data.joint_vel)
        self.robot.write_joint_state_to_sim(joint_pos, joint_vel)
        
        self.scene.reset()
        
        for _ in range(10):
            self.robot.set_joint_effort_target(zero_efforts)
            self.scene.write_data_to_sim()
            self.sim.step()
            self.scene.update(self.sim_dt)
        
        current_vel = self.robot.data.joint_vel[0, 0].item()
        current_pos = self.robot.data.joint_pos[0, 0].item()
        print(f"    Reset verification: pos={current_pos:.6f}, vel={current_vel:.6f}")

    def save_results(self):
        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
        summary_filename = f"armature_test_summary_{self.actuator_type}_{timestamp}.csv"
        summary_filepath = os.path.join(self.output_dir, summary_filename)
        
        with open(summary_filepath, 'w', newline='', encoding='utf-8') as csvfile:
            fieldnames = ['actuator_type', 'armature', 'avg_effort', 'avg_vel', 'avg_acc', 'avg_theoretical_acc', 'applied_effort']
            writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
            
            writer.writeheader()
            for row in self.test_results:
                writer.writerow(row)
        
        detail_filename = f"armature_test_detail_{self.actuator_type}_{timestamp}.csv"
        detail_filepath = os.path.join(self.output_dir, detail_filename)
        
        with open(detail_filepath, 'w', newline='', encoding='utf-8') as csvfile:
            fieldnames = ['armature', 'time', 'step', 'joint_effort',
                         'reaction_wrench_norm', 
                         'reaction_force_x', 'reaction_force_y', 'reaction_force_z',
                         'reaction_torque_x', 'reaction_torque_y', 'reaction_torque_z', 
                        #  'centrifugal_indicator',
                            # 'clean_reaction_tau',
                         'joint_vel', 'joint_acc', 
                        #  'joint_pos', 'applied_effort', 
                         'theoretical_acc']
            writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
            
            writer.writeheader()
            for row in self.detailed_data:
                writer.writerow(row)
        
        print(f"[INFO] Summary results saved to: {summary_filepath}")
        print(f"[INFO] Detailed results saved to: {detail_filepath}")
        return summary_filepath, detail_filepath

    def run_complete_test(self):
        print("\n" + "="*60)
        print("Starting Armature Parameter Identification Test")
        print("="*60)
        
        print("\n[INFO] Robot Physical Properties:")
        self.get_link_properties()
        self.get_joint_properties()
        self.get_geometry_properties()
        
        armature_values = [0.0, 1.0, 2.0, 3.0, 4.0, 5.0]
        
        for armature in armature_values:
            self.run_single_armature_test(armature)
            
            print(f"    Waiting for system stabilization...")
            time.sleep(1.0)
        
        self.save_results()
        
        print("\n" + "="*60)
        print("Armature Parameter Identification Test Completed!")
        print("="*60)

    def get_link_properties(self):
        masses = self.robot.data.default_mass
        inertias = self.robot.data.default_inertia 
        
        com_pos = self.robot.data.body_com_pos_b  
        body_names = self.robot.body_names
        
        print("\n[INFO] Link Properties:")
        for i, body_name in enumerate(body_names):
            mass = masses[0, i].item()
            inertia = inertias[0, i, :].cpu().numpy()
            com = com_pos[0, i, :].cpu().numpy()
            
            I_matrix = inertia.reshape(3, 3)
            
            print(f"  Body {i} ({body_name}):")
            print(f"    Mass: {mass:.6f} kg")
            print(f"    COM: [{com[0]:.6f}, {com[1]:.6f}, {com[2]:.6f}]")
            print(f"    Inertia matrix:")
            print(f"      [{I_matrix[0,0]:.6f}, {I_matrix[0,1]:.6f}, {I_matrix[0,2]:.6f}]")
            print(f"      [{I_matrix[1,0]:.6f}, {I_matrix[1,1]:.6f}, {I_matrix[1,2]:.6f}]")
            print(f"      [{I_matrix[2,0]:.6f}, {I_matrix[2,1]:.6f}, {I_matrix[2,2]:.6f}]")

    def get_joint_properties(self):
        joint_names = self.robot.joint_names
        
        joint_pos_limits = self.robot.data.soft_joint_pos_limits 
        joint_vel_limits = self.robot.data.soft_joint_vel_limits 
        
        print("\n[INFO] Joint Properties:")
        for i, joint_name in enumerate(joint_names):
            pos_limit_low = joint_pos_limits[0, i, 0].item()
            pos_limit_high = joint_pos_limits[0, i, 1].item()
            vel_limit = joint_vel_limits[0, i].item()
            
            print(f"  Joint {i} ({joint_name}):")
            print(f"    Position limits: [{pos_limit_low:.6f}, {pos_limit_high:.6f}]")
            print(f"    Velocity limit: {vel_limit:.6f}")
        
        actuator_names = list(self.robot.actuators.keys())
        print(f"\n[INFO] Actuator Properties:")
        for actuator_name, actuator in self.robot.actuators.items():
            print(f"  Actuator ({actuator_name}):")
            print(f"    Type: {type(actuator.cfg).__name__}")
            print(f"    Stiffness: {actuator.cfg.stiffness}")
            print(f"    Damping: {actuator.cfg.damping}")
            print(f"    Armature: {actuator.cfg.armature}")
            print(f"    Effort limit: {actuator.cfg.effort_limit_sim}")

    def get_geometry_properties(self):
        """获取几何属性（需要通过USD或Prim访问）"""
        try:
            import omni.isaac.core.utils.prims as prim_utils
            
            prim_path = self.robot.cfg.prim_path.replace("{ENV_REGEX_NS}", "/World/envs/env_0")
            
            articulation_prim = prim_utils.get_prim_at_path(prim_path)
            
            if articulation_prim:
                print("\n[INFO] Geometry Properties:")
                for child_prim in articulation_prim.GetChildren():
                    child_path = str(child_prim.GetPath())
                    prim_type = child_prim.GetTypeName()
                    
                    print(f"  Prim: {child_path}")
                    print(f"    Type: {prim_type}")
                    
                    if prim_type in ["Mesh", "Sphere", "Box", "Cylinder", "Capsule"]:
                        pass
        except ImportError:
            print("\n[WARNING] Cannot import omni.isaac.core.utils.prims for geometry properties")
        except Exception as e:
            print(f"\n[WARNING] Error getting geometry properties: {e}")

def create_actuator_config(actuator_type: str):
    if (actuator_type == "ideal"):
        return IdealPDActuatorCfg(
            joint_names_expr=[".*"],
            effort_limit_sim=1000.0,
            stiffness=0.0,
            damping=0.0, 
            armature=1.0, 
        )
    elif actuator_type == "implicit":
        return ImplicitActuatorCfg(
            joint_names_expr=[".*"],
            effort_limit_sim=1000.0,
            stiffness=0.0, 
            damping=0.0, 
            armature=1.0, 
        )
    else:
        raise ValueError(f"Unknown actuator type: {actuator_type}")


def main():
    sim_cfg = sim_utils.SimulationCfg(
        device=args_cli.device,
        dt=0.01, 
        render_interval=1,
        gravity=(0.0, 0.0, 0.0), 
    )
    sim = SimulationContext(sim_cfg)
    
    sim.set_camera_view((2.5, 0.0, 4.0), (0.0, 0.0, 2.0))
    
    scene_cfg = ArmatureTestSceneCfg(num_envs=args_cli.num_envs)
    scene_cfg.test_joint.actuators["test_actuator"] = create_actuator_config(args_cli.actuator_type)
    scene = InteractiveScene(scene_cfg)
    
    sim.reset()
    
    print("[INFO] Simulation environment setup completed...")
    
    test = ArmatureIdentificationTest(sim, scene, args_cli.actuator_type)
    test.run_complete_test()


if __name__ == "__main__":
    main()
    simulation_app.close()

[RandomOakForest]

Thank you for posting this. This is a great topic for our Discussions section. I'll move your post there for follow-up.