Featherstone Integrator vs Semi Implicit Integrator

[DINHQuangDung1999]

Hi everyone,

I am trying to import a hopper model as mujoco .xml format. I notice that when I use the Semi Implicit Integrator, the model gets broken apart, but the physics look nature. Meanwhile, using Featherstone integrator correctly initialize the model, but demonstrates chaotic and unexpected behavior. I tried changing the physics parameters, but so far it does not help. Do you know what is the reason behind this? I attach the code and the hopper model below.

import math
import os
import warp as wp
import warp.sim
import warp.sim.render

class Example:
    def __init__(self, stage_path="example_shapebox.usd", num_envs=4):
        self.num_envs = num_envs

        self.sim_time = 0.0
        fps = 60
        self.frame_dt = 1.0 / fps
        self.horizon = 300
        self.sim_substeps = 8
        self.sim_steps = self.sim_substeps * self.horizon
        self.sim_dt = self.frame_dt / self.sim_substeps

        ke = 2.0e4
        kd = 1.0e2
        kf = 1.0e1
        mu = 0.9
        re = 0.0
        margin = 1.0e0

        builder = wp.sim.ModelBuilder(up_vector=(0.0, 1.0, 0.0), gravity=-9.81)

        articulation_builder = wp.sim.ModelBuilder(up_vector=(0.0, 1.0, 0.0), gravity=-9.81)
        wp.sim.parse_mjcf(
            os.path.join("../assets/hopper.xml"),
            articulation_builder,
            density=1.0e4,
            stiffness=0.0,
            damping=2.0,
            contact_ke=1.0e3,
            contact_kd=1.0e2,
            contact_kf=1.0e1,
            contact_mu=0.9,
            contact_restitution=0.0,
            limit_ke=1.0e1,
            limit_kd=1.0e0,
            armature_scale=1.0,
            enable_self_collisions=False,
            # up_axis="Z",
        )
        articulation_builder.joint_axis_mode = [wp.sim.JOINT_MODE_FORCE] * len(articulation_builder.joint_axis_mode)
        articulation_builder.joint_q[3:6] = [0.0, 0.0, 0.0]
        
        builder.add_builder(
            articulation_builder,
            wp.transform([0.0, 0.0, 0.0], wp.quat_identity()),
            update_num_env_count=True,
            separate_collision_group=True,
        )
        self.model = builder.finalize(requires_grad = True)
        self.model.ground = True
        self.model.soft_contact_ke = ke
        self.model.soft_contact_kd = kd
        self.model.soft_contact_kf = kf
        self.model.soft_contact_mu = mu
        self.model.soft_contact_margin = margin
        self.model.soft_contact_restitution = re    # or ~0.1 if you want bounce

        self.integrator = wp.sim.FeatherstoneIntegrator(self.model)
        # self.integrator = wp.sim.SemiImplicitIntegrator()
        breakpoint()
        self.states = [self.model.state() for i in range(self.sim_steps + 1)]

        self.tape = wp.Tape()

        wp.sim.collide(self.model, self.states[0])

        self.use_cuda_graph = False
        if self.use_cuda_graph:
            with wp.ScopedCapture() as capture:
                self.simulate()
            self.graph = capture.graph

        self.renderer = None
        if stage_path:
            self.renderer = wp.sim.render.SimRenderer(path=stage_path, model=self.model, scaling=15.0)

    def simulate(self):
        for i in range(self.sim_steps):
            self.states[i].clear_forces()
            wp.sim.collide(self.model, self.states[i], requires_grad = True)
            self.integrator.simulate(self.model, self.states[i], self.states[i+1], self.sim_dt)

    def step(self):
        if self.use_cuda_graph:
            wp.capture_launch(self.graph)
        else:
            self.simulate()

    def render(self):
        if self.renderer is None:
            return

        for i in range(0, self.sim_steps + 1, self.sim_substeps):
            self.renderer.begin_frame(self.sim_time)
            self.renderer.render(self.states[i])
            self.renderer.end_frame()
            self.sim_time += self.frame_dt


if __name__ == "__main__":

    example = Example()

    example.step()
    example.render()
    # example.train()
    if example.renderer:
        example.renderer.save()

<mujoco model="hopper">
    <compiler angle="degree" coordinate="local" inertiafromgeom="true"/>
    <option integrator="RK4" timestep="0.01"/>
    <size njmax="500" nconmax="100" />
    <visual>
        <map znear="0.02" />
    </visual>
    <default class="main">
        <joint limited="true" armature="0.001" damping="1" />
        <geom condim="1" solimp="0.8 0.8 0.01 0.5 2" margin="0.001" material="geom" rgba="0.8 0.6 0.4 1" />
        <general ctrllimited="true" ctrlrange="-0.4 0.4" />
    </default>
    <asset>
        <texture type="skybox" builtin="gradient" rgb1="0.4 0.5 0.6" rgb2="0 0 0" width="100" height="600" />
        <texture type="cube" name="texgeom" builtin="flat" mark="cross" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" width="127" height="762" />
        <texture type="2d" name="texplane" builtin="checker" rgb1="0 0 0" rgb2="0.8 0.8 0.8" width="100" height="100" />
        <material name="MatPlane" texture="texplane" texrepeat="60 60" specular="1" shininess="1" reflectance="0.5" />
        <material name="geom" texture="texgeom" texuniform="true" />
    </asset>
    <worldbody>
        <geom name="floor" size="20 20 0.125" type="plane" condim="3" material="MatPlane" rgba="0.8 0.9 0.8 1" />
        <light pos="0 0 1.3" dir="0 0 -1" directional="true" cutoff="100" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1" />
        <body name="torso" pos="0 0 1.25">
            <joint name="rootx" pos="0 0 -1.25" axis="1 0 0" type="slide" limited="false" armature="0" damping="0" />
            <joint name="rootz" pos="0 0 0" axis="0 0 1" type="slide" ref="1.25" limited="false" armature="0" damping="0" />
            <joint name="rooty" pos="0 0 0" axis="0 1 0" limited="false" type="hinge" armature="0" damping="0" />
            <geom name="torso_geom" size="0.05 0.2" type="capsule" friction="0.9 0.005 0.0001" />
            <body name="thigh" pos="0 0 -0.2">
                <joint name="thigh_joint" pos="0 0 0" type="hinge" axis="0 -1 0" range="-0 60" />
                <geom name="thigh_geom" size="0.05 0.225" pos="0 0 -0.225" type="capsule" friction="0.9 0.005 0.0001" />
                <body name="leg" pos="0 0 -0.7">
                    <joint name="leg_joint" pos="0 0 0.25" type="hinge" axis="0 -1 0" range="-60 60" />
                    <geom name="leg_geom" size="0.04 0.25" type="capsule" friction="0.9 0.005 0.0001" />
                    <body name="foot" pos="0.0 0 -0.25">
                        <joint name="foot_joint" pos="0 0 0.0" type="hinge" axis="0 -1 0" range="-60 60" />
                        <geom name="foot_geom" size="0.06 0.195" pos="0.06 0 0.0" quat="0.707107 0 -0.707107 0" type="capsule" friction="2 0.005 0.0001" />
                    </body>
                </body>
            </body>
        </body>
    </worldbody>
    <actuator>
        <general ctrllimited="true" joint="thigh_joint" ctrlrange="-1 1" gear="150 0 0 0 0 0" />
        <general ctrllimited="true" joint="leg_joint" ctrlrange="-1 1" gear="150 0 0 0 0 0" />
        <general ctrllimited="true" joint="foot_joint" ctrlrange="-1 1" gear="150 0 0 0 0 0" />
    </actuator>
</mujoco>

[shi-eric]

warp.sim is deprecated and I don't think anyone from our team has the time to look into this question. Consider posing this question in the Newton discussions instead (assuming you can still observe the same differences there): https://github.com/newton-physics/newton/discussions