Franka URDF does not import properly

[e.xing]

Edit:

Looks like the visual meshes are just flipped, the collision bodies are correct. Would be nice to have a flip_visual_attachments: Switch Meshes from Z-up left-handed system to Y-up Right-handed coordinate system. option for wp.sim.parse_urdf(), like in isaacgym:

Screenshot (collision shapes)...

---

franka_panda.urdf

Screenshot (visual meshes wrong)...

xarm6_robot.urdf

Screenshot (works as intended)...

---

Code to repro...

Modified example_quadruped.py:

import math
import os

import numpy as np

import warp as wp
import warp.examples
import warp.sim
import warp.sim.render

wp.init()


# Taken from env/environment.py
def compute_env_offsets(num_envs, env_offset=(5.0, 0.0, 5.0), up_axis="Y"):
  # compute positional offsets per environment
  env_offset = np.array(env_offset)
  nonzeros = np.nonzero(env_offset)[0]
  num_dim = nonzeros.shape[0]
  if num_dim > 0:
      side_length = int(np.ceil(num_envs ** (1.0 / num_dim)))
      env_offsets = []
  else:
      env_offsets = np.zeros((num_envs, 3))
  if num_dim == 1:
      for i in range(num_envs):
          env_offsets.append(i * env_offset)
  elif num_dim == 2:
      for i in range(num_envs):
          d0 = i // side_length
          d1 = i % side_length
          offset = np.zeros(3)
          offset[nonzeros[0]] = d0 * env_offset[nonzeros[0]]
          offset[nonzeros[1]] = d1 * env_offset[nonzeros[1]]
          env_offsets.append(offset)
  elif num_dim == 3:
      for i in range(num_envs):
          d0 = i // (side_length * side_length)
          d1 = (i // side_length) % side_length
          d2 = i % side_length
          offset = np.zeros(3)
          offset[0] = d0 * env_offset[0]
          offset[1] = d1 * env_offset[1]
          offset[2] = d2 * env_offset[2]
          env_offsets.append(offset)
  env_offsets = np.array(env_offsets)
  min_offsets = np.min(env_offsets, axis=0)
  correction = min_offsets + (np.max(env_offsets, axis=0) - min_offsets) / 2.0
  if isinstance(up_axis, str):
      up_axis = "XYZ".index(up_axis.upper())
  correction[up_axis] = 0.0  # ensure the envs are not shifted below the ground plane
  env_offsets -= correction
  return env_offsets


class Example:
  def __init__(self, stage=None, num_envs=1, print_timers=True):
      self.num_envs = num_envs
      articulation_builder = wp.sim.ModelBuilder()
      wp.sim.parse_urdf(
          os.path.join(
              os.path.dirname(__file__),
              "assets/franka_description/robots/franka_panda.urdf",
              # "assets/xarm/xarm6_robot.urdf",
          ),
          articulation_builder,
          xform=wp.transform([0.0, 0.0, 0.0], wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), -math.pi * 0.5)),
          floating=False,
          density=1000,
          armature=0.1,
          stiffness=100,
          damping=40,
          # contact_ke=1.0e4,
          # contact_kd=1.0e2,
          # contact_kf=1.0e2,
          # contact_mu=1.0,
          # limit_ke=1.0e4,
          # limit_kd=1.0e1,
      )

      builder = wp.sim.ModelBuilder()

      self.sim_time = 0.0
      self.frame_dt = 1.0 / 60.0

      episode_duration = 0.5  # seconds
      self.episode_frames = int(episode_duration / self.frame_dt)

      self.sim_substeps = 16
      self.sim_dt = self.frame_dt / self.sim_substeps

      offsets = compute_env_offsets(num_envs)
      for i in range(num_envs):
          builder.add_builder(articulation_builder, xform=wp.transform(offsets[i], wp.quat_identity()))

          builder.joint_axis_mode = [wp.sim.JOINT_MODE_TARGET_POSITION] * len(builder.joint_axis_mode)
          builder.joint_q[-9:] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
          builder.joint_act[-9:] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
          # builder.joint_q[-9:] = [0, 0.1963, 0, -2.6180, 0, 2.9416, 0.7854, 0.035, 0.035]
          # builder.joint_act[-9:] = [0, 0.1963, 0, -2.6180, 0, 2.9416, 0.7854, 0.035, 0.035]

          # builder.joint_q[-12:] = [0.2, 0.4, -0.6, -0.2, -0.4, 0.6, -0.2, 0.4, -0.6, 0.2, -0.4, 0.6]
          # builder.joint_act[-12:] = [0.2, 0.4, -0.6, -0.2, -0.4, 0.6, -0.2, 0.4, -0.6, 0.2, -0.4, 0.6]

      np.set_printoptions(suppress=True)
      # finalize model
      self.model = builder.finalize()
      self.model.ground = False
      # self.model.gravity = 0.0

      # self.model.joint_attach_ke = 16000.0
      # self.model.joint_attach_kd = 200.0

      # self.integrator = wp.sim.XPBDIntegrator()
      # self.integrator = wp.sim.SemiImplicitIntegrator()
      self.integrator = wp.sim.FeatherstoneIntegrator(self.model)

      self.renderer = None
      if stage:
          self.renderer = wp.sim.render.SimRenderer(self.model, stage)

      self.print_timers = print_timers

      self.state_0 = self.model.state()
      self.state_1 = self.model.state()

      wp.sim.eval_fk(self.model, self.model.joint_q, self.model.joint_qd, None, self.state_0)

      self.use_graph = wp.get_device().is_cuda
      self.graph = None
      if self.use_graph:
          with wp.ScopedCapture() as capture:
              self.simulate()
          self.graph = capture.graph

  def simulate(self):
      for _ in range(self.sim_substeps):
          self.state_0.clear_forces()
          wp.sim.collide(self.model, self.state_0)
          self.integrator.simulate(self.model, self.state_0, self.state_1, self.sim_dt)
          self.state_0, self.state_1 = self.state_1, self.state_0

  def step(self):
      with wp.ScopedTimer("step", active=True, print=self.print_timers):
          if self.use_graph:
              wp.capture_launch(self.graph)
          else:
              self.simulate()
      self.sim_time += self.frame_dt

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

      with wp.ScopedTimer("render", active=True, print=self.print_timers):
          self.renderer.begin_frame(self.sim_time)
          self.renderer.render(self.state_0)
          self.renderer.end_frame()


if __name__ == "__main__":

  stage_path = "example_franka.usd"

  example = Example(stage_path, num_envs=1)

  for _ in range(example.episode_frames):
      example.step()
      example.render()

  if example.renderer:
      example.renderer.save()

[e.xing]

Would also be nice if wp.sim.parse_urdf() automatically resolved filename="package://... paths!

Currently editing URDFs by hand:

<mesh filename="package://franka_description/meshes/collision/link0.obj"/>

->

<mesh filename="../meshes/collision/link0.obj"/>

[e.xing]

Would also be nice to have a flag that loads torque ("effort) and velocity ("velocity") limits per joint from the URDF:

<joint>
  <limit lower="0", upper="0", effort="0", velocity="0">
</joint>

[Eric Heiden]

Hi @etaoxing,

Thank you for reporting these issues! We will have the URDF importer fixed in the next release. The visual meshes in the COLLADA format actually have the up_axis encoded, so by replacing this line with

m = trimesh.load(filename, force="mesh")

we can force trimesh to bake the transforms into the mesh when importing it.

We will also add support for the package:// handling in the next version.

Support for torque and velocity limits is on our ToDo list but will take a bit longer to implement for the different integrators.

[Eric Shi]

I believe this was fixed by @eric-heiden in db752a3. Please reopen the issue if this is not the case. Thanks!