import warp as wpimport newtonimport newton.examplesclass Example: def __init__(self, viewer, args): self.fps = 100 self.frame_dt = 1.0 / self.fps self.sim_time = 0.0 self.sim_substeps = 10 self.sim_dt = self.frame_dt / self.sim_substeps self.viewer = viewer self.args = args builder = newton.ModelBuilder() hx = 1.0 hy = 0.1 hz = 0.1 link_0 = builder.add_link() builder.add_shape_box(link_0, hx=hx, hy=hy, hz=hz) link_1 = builder.add_link() builder.add_shape_box(link_1, hx=hx, hy=hy, hz=hz) rot = wp.quat_from_axis_angle(wp.vec3(0.0, 0.0, 1.0), -wp.pi * 0.5) j0 = builder.add_joint_revolute( parent=-1, child=link_0, axis=wp.vec3(0.0, 1.0, 0.0), parent_xform=wp.transform(p=wp.vec3(0.0, 0.0, 5.0), q=rot), child_xform=wp.transform(p=wp.vec3(-hx, 0.0, 0.0), q=wp.quat_identity()), ) j1 = builder.add_joint_revolute( parent=link_0, child=link_1, axis=wp.vec3(0.0, 1.0, 0.0), parent_xform=wp.transform(p=wp.vec3(hx, 0.0, 0.0), q=wp.quat_identity()), child_xform=wp.transform(p=wp.vec3(-hx, 0.0, 0.0), q=wp.quat_identity()), ) builder.add_articulation([j0, j1], label="pendulum") builder.add_ground_plane() self.model = builder.finalize() self.solver = newton.solvers.SolverXPBD(self.model) self.state_0 = self.model.state() self.state_1 = self.model.state() self.control = self.model.control() newton.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, self.state_0) self.contacts = self.model.contacts() self.viewer.set_model(self.model) self.capture() def capture(self): if wp.get_device().is_cuda: with wp.ScopedCapture() as capture: self.simulate() self.graph = capture.graph else: self.graph = None def simulate(self): for _ in range(self.sim_substeps): self.state_0.clear_forces() self.viewer.apply_forces(self.state_0) self.model.collide(self.state_0, self.contacts) self.solver.step(self.state_0, self.state_1, self.control, self.contacts, self.sim_dt) 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 test_final(self): newton.examples.test_body_state( self.model, self.state_0, "pendulum links in correct area", lambda q, qd: abs(q[0]) < 1e-5 and abs(q[1]) < 1.0 and q[2] < 5.0 and q[2] > 0.0, [0, 1], ) def check_velocities(_, qd): check = abs(qd[0]) < 1e-4 and abs(qd[6]) < 1e-4 check = check and abs(qd[1]) < 10.0 and abs(qd[2]) < 5.0 and abs(qd[3]) < 10.0 and abs(qd[4]) < 10.0 return check newton.examples.test_body_state( self.model, self.state_0, "pendulum links have reasonable velocities", check_velocities, [0, 1], ) 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()if __name__ == "__main__": viewer, args = newton.examples.init() example = Example(viewer, args) newton.examples.run(example, args)