mmatl / urdfpy

Python parser for URDFs
http://urdfpy.readthedocs.io/
MIT License
246 stars 98 forks source link

Does this support primitives? #3

Open walchko opened 4 years ago

walchko commented 4 years ago

I just started to work with this an made a simple urdf with 4 links. I am using simple cylinders for the geometry:

<visual>
        <geometry>
            <cylinder length="52" radius="1"/>
        </geometry>
</visual>

however when I run it, I get the following error:

Traceback (most recent call last):
  File "./test.py", line 33, in <module>
    robot.show(cfg=config)
  File "/Users/kevin/pyvenv/cv/lib/python3.7/site-packages/urdfpy/urdf.py", line 3540, in show
    fk = self.visual_trimesh_fk(cfg=cfg)
  File "/Users/kevin/pyvenv/cv/lib/python3.7/site-packages/urdfpy/urdf.py", line 3197, in visual_trimesh_fk
    for mesh in visual.geometry.meshes:
  File "/Users/kevin/pyvenv/cv/lib/python3.7/site-packages/urdfpy/urdf.py", line 734, in meshes
    return self.geometry.meshes
  File "/Users/kevin/pyvenv/cv/lib/python3.7/site-packages/urdfpy/urdf.py", line 396, in meshes
    if len(self._meshes) == 0:
TypeError: object of type 'NoneType' has no len()

I am using python 3.7 on macOS 10.15 and just installed urdfpy

Python

#!/usr/bin/env python

from urdfpy import URDF
from math import pi

def deg2rad(x):
    return x*pi/180

robot = URDF.load("kevin.urdf")

config = {
    "joint1": deg2rad(45),
    "joint2": deg2rad(45),
    "joint3": deg2rad(90),
    "joint4": deg2rad(-45)
}

fk = robot.link_fk(cfg=config)

for k in robot.links:
    print('-'*25)
    print(k.name)
    print(fk[k])

for j in robot.joints:
    print('-'*25)
    print('{}: {} to {}'.format(j.name, j.parent, j.child))
    # print(j.name)
    # print(dir(j))
    # print(j.origin)
    # print(dir(j.origin))

robot.show(cfg=config)

urdf

<?xml version="1.0"?>
  <robot name="quad">
    <material name="red">
      <color rgba="1 0 0 1" />
    </material>

    <link name="body">
    <visual>
        <geometry>
            <cylinder length="10" radius="0.5"/>
        </geometry>
        <material name="red"/>
    </visual>
    </link>

    <link name="coxa">
    <visual>
        <geometry>
            <cylinder length="52" radius="1"/>
        </geometry>
    </visual>
    </link>

    <link name="femur">
    <visual>
        <geometry>
            <cylinder length="89" radius="1"/>
        </geometry>
    </visual>
    </link>

    <link name="tibia">
    <visual>
        <geometry>
            <cylinder length="90" radius="1"/>
        </geometry>
    </visual>
    </link>

    <link name="tarsus">
    <visual>
        <geometry>
            <cylinder length="95" radius="1"/>
        </geometry>
    </visual>
    </link>
    <link name="foot"/>

    <joint name="joint1" type="revolute">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <parent link="body"/>
      <child link="coxa"/>
      <axis xyz="0 0 1.0"/>
      <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
    </joint>

    <joint name="joint2" type="revolute">
      <origin xyz="52 0 0" rpy="-1.570796 0 0"/>
      <parent link="coxa"/>
      <child link="femur"/>
      <axis xyz="0 0 1.0"/>
      <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
    </joint>

    <joint name="joint3" type="revolute">
      <origin xyz="89 0 0" rpy="0 0 0"/>
      <parent link="femur"/>
      <child link="tibia"/>
      <axis xyz="0 0 1.0"/>
      <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
    </joint>

    <joint name="joint4" type="revolute">
      <origin xyz="90 0 0" rpy="0 0 0"/>
      <parent link="tibia"/>
      <child link="tarsus"/>
      <axis xyz="0 0 1.0"/>
      <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
    </joint>

    <joint name="joint5" type="fixed">
      <origin xyz="95 0 0" rpy="0 0 0"/>
      <parent link="tarsus"/>
      <child link="foot"/>
      <axis xyz="0 0 1.0"/>
      <limit effort="150.0" lower="-6.28318530718" upper="6.28318530718" velocity="3.15"/>
    </joint>

  </robot>
sthoduka commented 4 years ago

@walchko, this PR fixes your issue