Newton 为工业机器人增加了富含接触的操纵和移动能力

import newton​

# Import the articulation model from USD​
builder = newton.ModelBuilder()​

# Register attributes to be parsed specific to Kamino
newton.solvers.SolverKamino.register_custom_attributes(builder)

# Import USD asset
builder.add_usd("robot.usd")​

# Finalize the model (upload to GPU)​
model = builder.finalize()​

# Create Kamino solver​
solver = newton.solvers.SolverKamino(model)​

# Create state and control objects​
state_0 = model.state()​
state_1 = model.state()​
control = model.control()​
contacts = model.contacts()​

# Simulation loop​
for i in range(num_steps):​
state_0.clear_forces()​
model.collide(state_0, contacts)

      # Forward dynamics​
solver.step(state_0, state_1,​
control, contacts, sim_dt​)​

    # Swap states​
state_0, state_1 = state_1, state_0  ​

from isaaclab.sim import SimulationCfg
from isaaclab_newton.physics import NewtonCfg, MJWarpSolverCfg

# Configure Newton MJWarp simulation for the Franka Cube Env
FrankaCubeEnvCfg.solver_cfg = MJWarpSolverCfg(
    solver="newton",
    integrator="implicitfast",
    njmax=2000,
    nconmax=1000,
    impratio=1000.0,
    cone="elliptic",
    update_data_interval=2,
    iterations=20,
    ls_iterations=100,
    ls_parallel=True,
)

FrankaCubeEnvCfg.newton_cfg = NewtonCfg(
    solver_cfg=FrankaCubeEnvCfg.solver_cfg,
    num_substeps=2,
    debug_mode=False,
)

FrankaCubeEnvCfg.sim = SimulationCfg(
    dt=1 / 120,
    render_interval=FrankaCubeEnvCfg.decimation,
    physics=FrankaCubeEnvCfg.newton_cfg,
)

import newton
from newton.geometry import HydroelasticSDF

# --- 1. Shape configuration: enable hydroelastic contact ---
shape_cfg = newton.ModelBuilder.ShapeConfig(
    mu=1.0,             # friction coefficient
    gap=0.01,           # contact detection margin [m]
    ke=5.0e4,           # elastic contact stiffness (MJWarp fallback)
    kd=5.0e2,           # contact damping
    kh=1.0e11,          # hydroelastic stiffness [Pa/m] — controls
                        # pressure vs. penetration across the contact patch
)

# --- 2. Build SDF on each collision mesh ---
# Precompute a sparse signed-distance field so Newton can find
# sub-voxel contact surfaces via marching cubes at runtime.
for mesh in assembly_meshes:
    mesh.build_sdf(
        max_resolution=128,                # SDF grid resolution
        narrow_band_range=(-0.01, 0.01),   # ±10 mm band around surface
        margin=shape_cfg.gap,
    )

# --- 3. Mark shapes as hydroelastic ---
# When both shapes in a colliding pair carry this flag, Newton
# routes them through the SDF-hydroelastic pipeline instead of
# MJWarp's native point-contact solver.
for shape_idx in range(builder.shape_count):
    builder.shape_flags[shape_idx] |= newton.ShapeFlags.HYDROELASTIC

# --- 4. Create the collision pipeline with hydroelastic config ---
collision_pipeline = newton.CollisionPipeline(
    model,
    reduce_contacts=True,       # contact-reduction for stable solving
    broad_phase="explicit",     # precomputed shape pairs (few shapes)
    sdf_hydroelastic_config=HydroelasticSDF.Config(
        output_contact_surface=False,   # skip surface mesh export

    ),
)

# --- 5. Simulation loop (unchanged from standard Newton) ---
# The solver receives distributed contact patches transparently.
collision_pipeline.collide(state, contacts)
solver.step(state_0, state_1, control, contacts, dt)

import warp as wp
import newton
from newton.solvers import SolverMuJoCo, SolverVBD

# --- Universe A: MuJoCo rigid-body robot ---
robot_model = robot_builder.finalize()

mj_solver = SolverMuJoCo(
    robot_model,
    solver="newton",
    integrator="implicitfast",
    cone="elliptic",
    iterations=20,
    ls_iterations=10,
    ls_parallel=True,
    impratio=1000.0,
)

robot_state_0 = robot_model.state()
robot_state_1 = robot_model.state()
control = robot_model.control()
mj_collision_pipeline = newton.CollisionPipeline(
    robot_model,
    reduce_contacts=True,
    broad_phase="explicit",
)
mj_contacts = mj_collision_pipeline.contacts()

# --- Universe B: VBD deformable cable ---
cable_builder = newton.ModelBuilder()

cable_builder.add_rod(
    positions=cable_points,          # polyline vertices [m]
    quaternions=cable_quats,         # parallel-transport frames
    radius=0.003,                    # cable cross-section radius [m]
    stretch_stiffness=1e12,          # EA [N]
    bend_stiffness=3.0,              # EI [N*m^2]
    stretch_damping=1e-3,
    bend_damping=1.0,
)

# --- Proxy bodies: robot links mirrored into VBD ---
for body_id in proxy_body_ids:
    # Effective mass: reflects the inertia of the full articulated
    # chain when applicable, optionally scaled for coupling stability.
    proxy_id = cable_builder.add_body(
        xform=robot_state_0.body_q[body_id],
        mass=effective_mass[body_id],
    )
    for shape in shapes_on_body(robot_model, body_id):
        cable_builder.add_shape(body=proxy_id, **shape)

    robot_to_vbd[body_id] = proxy_id

cable_model = cable_builder.finalize()

vbd_solver = SolverVBD(
    cable_model,
    iterations=10,
)

vbd_state_0 = cable_model.state()
vbd_state_1 = cable_model.state()
vbd_control = cable_model.control()
vbd_collision_pipeline = newton.CollisionPipeline(cable_model)
vbd_contacts = vbd_collision_pipeline.contacts()

proxy_forces = wp.zeros(robot_model.body_count, dtype=wp.spatial_vector)
coupling_forces_cache = wp.zeros_like(proxy_forces)


@wp.kernel
def sync_proxy_state(
    robot_ids: wp.array(dtype=int),
    proxy_ids: wp.array(dtype=int),
    src_body_q: wp.array(dtype=wp.transform),
    src_body_qd: wp.array(dtype=wp.spatial_vector),
    dst_body_q: wp.array(dtype=wp.transform),
    dst_body_qd: wp.array(dtype=wp.spatial_vector),
    proxy_forces: wp.array(dtype=wp.spatial_vector),
    body_inv_mass: wp.array(dtype=float),
    body_inv_inertia: wp.array(dtype=wp.mat33),
    gravity: wp.vec3,
    dt: float,
):
    i = wp.tid()
    rid = robot_ids[i]
    pid = proxy_ids[i]

    # Copy pose and velocity from robot to proxy
    dst_body_q[pid] = src_body_q[rid]
    qd = src_body_qd[rid]

    # Undo coupling forces + gravity on proxy velocity
    f = proxy_forces[rid]
    delta_v = dt * body_inv_mass[pid] * wp.spatial_top(f)
    r = wp.transform_get_rotation(dst_body_q[pid])
    delta_w = dt * wp.quat_rotate(r, body_inv_inertia[pid] * wp.quat_rotate_inv(r, wp.spatial_bottom(f)))
    qd = qd - wp.spatial_vector(delta_v + dt * body_inv_mass[pid] * gravity, delta_w)

    dst_body_qd[pid] = qd


# --- Coupled step (staggered, one-step lag) ---

# Step 1 -- Apply lagged VBD-to-MuJoCo wrenches

robot_state_0.clear_forces()
coupling_forces_cache.assign(proxy_forces)
robot_state_0.body_f.assign(robot_state_0.body_f + coupling_forces_cache)

# Step 2 -- Advance MuJoCo (rigid-body robot)
mj_collision_pipeline.collide(robot_state_0, mj_contacts)
mj_solver.step(robot_state_0, robot_state_1, control, mj_contacts, dt)
robot_state_0, robot_state_1 = robot_state_1, robot_state_0

# Step 3 + 4 -- Sync proxy poses/velocities and undo coupling forces (single kernel)
wp.launch(
    sync_proxy_state,
    dim=len(proxy_body_ids),
    inputs=[
        robot_ids_wp, proxy_ids_wp,
        robot_state_0.body_q, robot_state_0.body_qd,
        vbd_state_0.body_q, vbd_state_0.body_qd,
        coupling_forces_cache,
        cable_model.body_inv_mass, cable_model.body_inv_inertia,
        gravity, dt,
    ],
)

# Step 5 -- Advance VBD (cable deformation + cable-proxy contacts)
vbd_collision_pipeline.collide(vbd_state_0, vbd_contacts)
vbd_solver.step(vbd_state_0, vbd_state_1, vbd_control, vbd_contacts, dt)

# Step 6 -- Harvest contact wrenches from proxy bodies (applied at next step)
proxy_forces = harvest_proxy_wrenches(vbd_solver, vbd_state_1, vbd_contacts, dt)

vbd_state_0, vbd_state_1 = vbd_state_1, vbd_state_0

标签