Giter Site home page Giter Site logo

iory / scikit-robot Goto Github PK

View Code? Open in Web Editor NEW
114.0 10.0 23.0 2.47 MB

A Flexible Framework for Robot visualization and programming in Python

Home Page: https://scikit-robot.readthedocs.io/

License: MIT License

Python 100.00%
robot kinematics ros visualization flexible-framework python robot-control motion-planning path-planning path-planner

scikit-robot's Introduction

scikit-robot: A Flexible Framework for Robot visualization and programming in Python

PyPI Build Status

Scikit-Robot is a lightweight pure-Python library for robotic kinematics, motion planning, visualization and control.

Installation

You may need to install some dependencies by apt-get install:

sudo apt-get update
sudo apt-get install libspatialindex-dev freeglut3-dev libsuitesparse-dev libblas-dev liblapack-dev

Then,

pip install scikit-robot

If you would like to use Pybullet Interface and open3d for mesh simplification,

pip install scikit-robot[all]

Features

scikit-robot's People

Contributors

708yamaguchi avatar hiroishida avatar iory avatar kosuke55 avatar ktro2828 avatar nakane11 avatar ojh6404 avatar rarilurelo avatar rkoyama1623-2021 avatar t141 avatar wkentaro avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

scikit-robot's Issues

RuntimeError when trying to use the viewer

RuntimeError occurred when I tried to use the viewer as follows.

$ python3
>>> import skrobot
>>> viewer = skrobot.viewers.TrimeshSceneViewer()
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
  File "/home/kanazawa/.local/lib/python3.8/site-packages/skrobot/viewers/__init__.py", line 9, in __init__
    raise RuntimeError('TrimeshSceneViewer cannot be initialized. '
RuntimeError: TrimeshSceneViewer cannot be initialized. This issue happens when the X window system is not running.

I understand that it is in the exception handling here

try:
from ._trimesh import TrimeshSceneViewer
except TypeError:
# trimesh.viewer.SceneViewer can have function type.
class TrimeshSceneViewer(object):
def __init__(self, *args, **kwargs):
raise RuntimeError('TrimeshSceneViewer cannot be initialized. '
'This issue happens when the X window system '
'is not running.')
, but how can I avoid the error?

I believe that the X Windows System is running because I can check the operation with xeyes.

ValueError: zero-size array to reduction operation minimum which has no identity for pyrender viewer

python robot_models.py --viewer pyrender
/home/iory/src/github.com/iory/scikit-robot/skrobot/model/robot_model.py:1727: UserWarning: texture specified in URDF is not supported
warnings.warn(
Traceback (most recent call last):
File "robot_models.py", line 66, in
main()
File "robot_models.py", line 56, in main
viewer.set_camera(angles=[np.deg2rad(30), 0, 0])
File "/home/iory/src/github.com/iory/scikit-robot/skrobot/viewers/_pyrender.py", line 168, in set_camera
self.scene.bounds, fov=fov, rotation=rotation,
File "/home/iory/.local/lib/python3.8/site-packages/pyrender/scene.py", line 226, in bounds
corners_local = trimesh.bounds.corners(mesh.bounds)
File "/home/iory/.local/lib/python3.8/site-packages/pyrender/mesh.py", line 90, in bounds
bounds[0] = np.minimum(bounds[0], p.bounds[0])
File "/home/iory/.local/lib/python3.8/site-packages/pyrender/primitive.py", line 280, in bounds
self._bounds = self._compute_bounds()
File "/home/iory/.local/lib/python3.8/site-packages/pyrender/primitive.py", line 450, in _compute_bounds
bounds = np.array([np.min(self.positions, axis=0),
File "<array_function internals>", line 200, in amin
File "/home/iory/.local/lib/python3.8/site-packages/numpy/core/fromnumeric.py", line 2946, in amin
return _wrapreduction(a, np.minimum, 'min', axis, None, out,
File "/home/iory/.local/lib/python3.8/site-packages/numpy/core/fromnumeric.py", line 86, in _wrapreduction
return ufunc.reduce(obj, axis, dtype, out, **passkwargs)
ValueError: zero-size array to reduction operation minimum which has no identity

TODO lists

  • Add document everyone
  • Update inverse kinematics (nullspace + additional_jacobian) @iory
  • Refactoring code @iory
  • OpenGL viewer (trimesh + urdfpy) @wkentaro
  • Collision check (trimesh) @wkentaro
  • Create demo @iory

Can't solve ik with assoc axis copy_worldcoords()

import skrobot
from skrobot.coordinates import CascadedCoords

robot = skrobot.models.PR2()
robot.reset_pose()
viewer = skrobot.viewers.TrimeshSceneViewer()
viewer.add(robot)
viewer.show()

assoc_coords = CascadedCoords(pos=(0.1, 0, 0))
robot.larm.end_coords.parent.assoc(assoc_coords, relative_coords="local")
hoge = skrobot.model.Axis.from_cascoords(assoc_coords)
robot.larm.end_coords.parent.assoc(hoge, relative_coords="world", force=True)
viewer.add(hoge)

robot.larm.inverse_kinematics(assoc_coords, move_target=assoc_coords)



assoc_coords = CascadedCoords()
assoc_coords.newcoords(robot.larm.end_coords.copy_worldcoords())
robot.larm.end_coords.parent.assoc(assoc_coords, 'world')

robot.larm.inverse_kinematics(assoc_coords, move_target=assoc_coords)
robot.larm.inverse_kinematics(assoc_coords.copy_worldcoords(), move_target=assoc_coords)


assoc_coords_ = robot.larm.end_coords.copy_worldcoords().translate([0.1, 0, 0])
assoc_coords_axis = skrobot.model.Axis()
assoc_coords_axis.newcoords(assoc_coords.copy_worldcoords())
robot.larm.end_coords.parent.assoc(assoc_coords_axis, 'world')

robot.larm.inverse_kinematics(assoc_coords_axis, move_target=assoc_coords_axis)
robot.larm.inverse_kinematics(assoc_coords_axis.copy_worldcoords(), move_target=robot.larm.end_coords)


assoc_coords_axis = skrobot.model.Axis(pos=[0.1, 0, 0])
robot.larm.end_coords.parent.assoc(assoc_coords_axis, 'local')

robot.larm.inverse_kinematics(assoc_coords_axis, move_target=assoc_coords_axis)
robot.larm.inverse_kinematics(assoc_coords_axis.copy_worldcoords(), move_target=assoc_coords_axis) # bad case
robot.larm.inverse_kinematics(assoc_coords_axis.copy_worldcoords(), move_target=robot.larm.end_coords)
robot.larm.inverse_kinematics(assoc_coords_axis.copy_worldcoords(), move_target=robot.larm.end_coords.parent)

robot.larm.inverse_kinematics(robot.larm.end_coords.parent, move_target=robot.larm.end_coords.parent)
robot.larm.inverse_kinematics(robot.larm.end_coords.parent.copy_worldcoords(), move_target=robot.larm.end_coords.parent)

viewer fails to visualize mulitple elements

problem description

I want to visualize the following model.

<?xml version="1.0" ?>
<robot name="myrobot">
   <link name="base_link">
      <visual>
         <geometry>
            <box size="0.5 0.5 0.1"/>
         </geometry>
         <origin rpy="0.0 -0.0 0.0" xyz="0.0 0.0 0.0"/>
      </visual>
      <visual>
         <geometry>
            <box size="0.5 0.5 0.1"/>
         </geometry>
         <origin rpy="0.0 -0.0 0.0" xyz="0.0 0.0 1.0"/>
      </visual>
   </link>
</robot>

where the model is consists of two box with 1m distance. Let me call above xml file as test.xml then I tried to visualize it by the following code:

import skrobot
robot = skrobot.models.urdf.RobotModelFromURDF(urdf_file="./test.xml")
link = robot.base_link
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480))
viewer.add(robot)
viewer.show()

However, the result was like the image below, which shows that only single elements is visualized.

problem is not in loading multiple urdf mesh

I firstly thought that there is a bug while loading urdf link that has multiple visual elements, but found that there is no such bag by the following trial in interpreter. The result shows that the two visual element is properly loaded and passed to the trimesh scene. Also, we can confirm that two visual elements has 1m distance each other. (see the z component of the vertices).

In [2]: viewer.scene.geometry
Out[2]: 
OrderedDict([('140623407120144',
              <trimesh.Trimesh(vertices.shape=(8, 3), faces.shape=(12, 3))>),
             (u'140623407120144:IDQCEYQX9PJF',
              <trimesh.Trimesh(vertices.shape=(8, 3), faces.shape=(12, 3))>)])
In [4]: viewer.scene.geometry.values()[0].vertices
Out[4]: 
TrackedArray([[-0.25, -0.25, -0.05],
              [-0.25, -0.25,  0.05],
              [-0.25,  0.25, -0.05],
              [-0.25,  0.25,  0.05],
              [ 0.25, -0.25, -0.05],
              [ 0.25, -0.25,  0.05],
              [ 0.25,  0.25, -0.05],
              [ 0.25,  0.25,  0.05]])
In [5]: viewer.scene.geometry.values()[1].vertices
Out[5]: 
TrackedArray([[-0.25, -0.25,  0.95],
              [-0.25, -0.25,  1.05],
              [-0.25,  0.25,  0.95],
              [-0.25,  0.25,  1.05],
              [ 0.25, -0.25,  0.95],
              [ 0.25, -0.25,  1.05],
              [ 0.25,  0.25,  0.95],
              [ 0.25,  0.25,  1.05]])

problem seems not in trimesh side

Next I thought it might be a problem in the trimesh side. So, I tried the following snippets to check it, which directly corresponding the geometries of the above metioned xml file.

import skrobot
import trimesh
import trimesh.viewer
import numpy as np

robot = skrobot.models.urdf.RobotModelFromURDF(urdf_file="./test.xml")
link = robot.base_link
mesh_list = link.visual_mesh
scene = trimesh.Scene()
scene.add_geometry(mesh_list)
scene.show()

The result was the following, which successfully show the two boxes and implies bug is not in the trimesh side.

How to save image

I tried to save image by the following way, but I this causes segmentation fault at glGetIntegerv inside
BufferManager.__init__.

cc. @708yamaguchi

Do you have any idea to save image?

import skrobot
import time
import numpy as np

robot = skrobot.models.PR2()
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480))
viewer.add(robot)
viewer.show()
time.sleep(3.0)

png_byte = viewer.save_image("hoge.png")

The output is

/usr/lib/python3/dist-packages/requests/__init__.py:89: RequestsDependencyWarning: urllib3 (1.26.9) or chardet (3.0.4) doesn't match a supported version!
  warnings.warn("urllib3 ({}) or chardet ({}) doesn't match a supported "
/home/h-ishida/.local/lib/python3.8/site-packages/skrobot/model/robot_model.py:1691: UserWarning: texture specified in URDF is not supported
  warnings.warn(
Segmentation fault (core dumped)

for your information the output of pip3 freeze is
freeze.txt

how to force set the robot's pose to ceratin pose

Rather than setting pose relatively using .rotate and .translate, I sometimes want to force set the robot's pose to a cerain pose. Is there any way to do this?

My attempt was like below, where I used setter method .translation and .rotation. However as you can see from the result, setting these does not affect rarm_end_coords, which is connected to robot_model.

import skrobot
import numpy as np
from skrobot.coordinates.math import rpy_matrix

robot_model = skrobot.models.urdf.RobotModelFromURDF(urdf_file=skrobot.data.pr2_urdfpath())
rarm_end_coords = skrobot.coordinates.CascadedCoords(
        parent=robot_model.r_gripper_tool_frame, 
        name='rarm_end_coords')

print(rarm_end_coords.worldpos())
robot_model.translation = np.array([1.0, 1.0, 1.0])
robot_model.rotation = rpy_matrix(0.3, 0.3, 0.3)
print(rarm_end_coords.worldpos())

the output was

[ 0.951    -0.188     0.790675]
[ 0.951    -0.188     0.790675]

AttributeError in loading urdf model if type of transmission tag is written along with transmission instead of as a child tag

If type is written as a child tag of transmission, as in the first figure, there's no error in loading robot model with RobotModelFromURDF.

unnamed

But if type is written along with transmission as shown in the below figure (Default layout from jsk_model_tools?), type can't be found in this case and return None, leading error in loading urdf model.

unnamed (1)

Error of loading is shown below.

unnamed (3)

Is changing the non-root cascaded coords allowed?

I expected new and co2's worldcoords match, but they don't.

import numpy as np
from skrobot.coordinates import CascadedCoords, Coordinates
np.random.seed(0)

co1 = CascadedCoords(pos = [0.1, 0.2, 0.3])
co2 = CascadedCoords()
co1.assoc(co2)
new = Coordinates(np.random.randn(3))
co2.newcoords(new)
print(new)
print(co2)

output

#<Coordinates 0x7f3010dd8700 1.764 0.400 0.979 / 0.0 -0.0 0.0>
#<CascadedCoords 0x7f3010ebf3a0 1.864 0.600 1.279 / 0.0 -0.0 0.0>

Looks like a bug, but I'm not certain.
Is changing the child coords allowed? If so, the above behavior is itended?

jacobian for irrelevant move_target must be 0, but it isn't

In the following example, move_target is not assoc-ed to any link of the robot, in other words, it is "floating" in the world. Therefore, the all jacobian's elements must be 0, because any joint belongs to link_list does not affect the position of move_target.

import skrobot
from skrobot.coordinates import CascadedCoords

robot_model = skrobot.models.urdf.RobotModelFromURDF(
    urdf_file=skrobot.data.pr2_urdfpath())
robot_model.init_pose()
link_idx_table = {}
for link_idx in range(len(robot_model.link_list)):
    name = robot_model.link_list[link_idx].name
    link_idx_table[name] = link_idx
base_link = robot_model.link_list[0]

link_names = ["r_shoulder_pan_link", "r_shoulder_lift_link",
              "r_upper_arm_roll_link", "r_elbow_flex_link",
              "r_forearm_roll_link", "r_wrist_flex_link",
              "r_wrist_roll_link"]

link_list = [robot_model.link_list[link_idx_table[lname]] for lname in link_names]

move_target = CascadedCoords()
J = robot_model.calc_jacobian_from_link_list(
        move_target, link_list, transform_coords=base_link)
print(J)

However, the resultant Jacobian J in the following code is

[[-0.188    -0.790675 -0.       -0.790675 -0.       -0.790675 -0.      ]
 [ 0.05      0.        0.790675  0.        0.790675  0.       -0.790675]
 [ 0.        0.05      0.188     0.45      0.188     0.771    -0.188   ]]

so it seems to buggy.

Fail to pip install scikit-robot

When I tried to install scikit-robot like

$ pip install scikit-robot

The outputs were like this.

Collecting scikit-robot
  Using cached scikit-robot-0.0.9.tar.gz (703 kB)
Collecting cached-property
  Using cached cached_property-1.5.2-py2.py3-none-any.whl (7.6 kB)
Collecting cvxopt
  Using cached cvxopt-1.2.5-cp36-cp36m-manylinux1_x86_64.whl (11.6 MB)
Requirement already satisfied: future in /home/shmpwk/venv/lib/python3.6/site-packages (from scikit-robot) (0.18.2)
Collecting gdown
  Using cached gdown-3.12.2.tar.gz (8.2 kB)
  Installing build dependencies ... done
  Getting requirements to build wheel ... error
  ERROR: Command errored out with exit status 1:
   command: /home/shmpwk/venv/bin/python3 /home/shmpwk/venv/lib/python3.6/site-packages/pip/_vendor/pep517/_in_process.py get_requires_for_build_wheel /tmp/tmpp777k68r
       cwd: /tmp/pip-install-kkua2b1c/gdown
  Complete output (10 lines):
  Traceback (most recent call last):
    File "/home/shmpwk/venv/lib/python3.6/site-packages/pip/_vendor/pep517/_in_process.py", line 280, in <module>
      main()
    File "/home/shmpwk/venv/lib/python3.6/site-packages/pip/_vendor/pep517/_in_process.py", line 263, in main
      json_out['return_val'] = hook(**hook_input['kwargs'])
    File "/home/shmpwk/venv/lib/python3.6/site-packages/pip/_vendor/pep517/_in_process.py", line 108, in get_requires_for_build_wheel
      backend = _build_backend()
    File "/home/shmpwk/venv/lib/python3.6/site-packages/pip/_vendor/pep517/_in_process.py", line 99, in _build_backend
      obj = getattr(obj, path_part)
  AttributeError: module 'setuptools.build_meta' has no attribute '__legacy__'
  ----------------------------------------
ERROR: Command errored out with exit status 1: /home/shmpwk/venv/bin/python3 /home/shmpwk/venv/lib/python3.6/site-packages/pip/_vendor/pep517/_in_process.py get_requires_for_build_wheel /tmp/tmpp777k68r Check the logs for full command output.
WARNING: You are using pip version 20.2; however, version 20.2.4 is available.
You should consider upgrading via the '/home/shmpwk/venv/bin/python3 -m pip install --upgrade pip' command.

My environment is :

  • Ubuntu 18.04
  • venv, python3.6
  • pip 20.2

Enhancement: create urdf

cc: @hiroya1224

import numpy as np

from skrobot.coordinates.math import random_quaternion
from skrobot.coordinates.math import random_translation
from skrobot.coordinates.math import quaternion2rpy
from skrobot.model import RobotModel
from skrobot.model import RotationalJoint
from skrobot.model import Cylinder
from skrobot.coordinates import Coordinates
from skrobot.viewers import TrimeshSceneViewer


class JointRelative(object):
    def __init__(self,
                 reference_jointname, target_jointname,
                 relative_position, relative_rotation):
        # frame_id of reference_joint and target_joint
        self.reference_jointname = reference_jointname
        self.target_jointname = target_jointname
        # target_joint's position and rotation w.r.t. reference_joint
        # 3D vector
        self.relative_position = relative_position
        # quaternion (wxyz order) or rotation
        self.relative_rotation = relative_rotation


joint_relatives = [
    JointRelative("joint{}".format(0), "joint{}".format(1),
                      [1, 0, 0],
                      [1, 0, 0, 0],
                      ),
    JointRelative("joint{}".format(1), "joint{}".format(2),
                      [1, 0, 0],
                      [1, 0, 0, 0],
                      )
]


def generate_urdf(joint_relatives):
    urdf_template = """
<robot name="simple_robot">
  {links}
  {joints}
</robot>
"""

    link_template = """
  <link name="{name}">
    <visual>
      <geometry>
        <cylinder length="{length}" radius="0.1"/>
      </geometry>
      <origin rpy="{rpy}" xyz="{xyz}"/>
    </visual>
  </link>
"""

    joint_template = """
  <joint name="{name}" type="revolute">
    <parent link="{parent}"/>
    <child link="{child}"/>
    <origin xyz="{xyz}" rpy="{rpy}"/>
    <axis xyz="0 1 0"/>
    <limit effort="10" lower="-3.141592653589793" upper="3.141592653589793" velocity="10" />
  </joint>
"""

    links = []
    joints = []

    # Add root link
    cylinder_rpy = '0.0 1.5707963267948966 0.0'
    links.append(link_template.format(name='base_link',
                                      length=0.01,
                                      xyz='0.01 0 0',
                                      rpy=cylinder_rpy))
    joints.append(joint_template.format(
        name=f"joint0",
        parent='base_link',
        child='link_joint0',
        xyz="0 0 0",
        rpy="0 0 0",
    ))

    for i, joint_relative in enumerate(joint_relatives):
        # Add link
        length = np.linalg.norm(joint_relative.relative_position)
        links.append(link_template.format(
            name='link_' + joint_relative.reference_jointname,
            length=length,
            xyz=" ".join(map(str, [length / 2.0, 0, 0])),
            # xyz=" ".join(map(str, [0, 0, 0])),
            rpy=cylinder_rpy))

        # Add joint
        parent_link = 'link_' + joint_relative.reference_jointname
        joints.append(joint_template.format(
            name=f"joint{i + 1}",
            parent=parent_link,
            child='link_' + joint_relative.target_jointname,
            xyz=" ".join(map(str, joint_relative.relative_position)),
            rpy=" ".join(map(str, quaternion2rpy(joint_relative.relative_rotation)[0])),
        ))

    links.append(link_template.format(
        name='link_' + joint_relative.target_jointname,
        length=0.01,
        xyz="0.005 0 0",
        rpy=cylinder_rpy))

    urdf_str = urdf_template.format(links="\n".join(links), joints="\n".join(joints))
    return urdf_str


urdf_str = generate_urdf(joint_relatives)
out_urdfpath = '/tmp/robot.urdf'
with open(out_urdfpath, 'w') as f:
    f.write(urdf_str)

robot_model = RobotModel()
robot_model.load_urdf_file(open(out_urdfpath))
viewer = TrimeshSceneViewer()
viewer.add(robot_model)
viewer.show()
robot_model.joint0.joint_angle(0.4)
robot_model.joint1.joint_angle(-0.5)

robot_model.joint0.joint_angle(0.4)
robot_model.joint1.joint_angle(-0.5)
robot_model.inverse_kinematics(
    target_coords=Coordinates(pos=[1.9, 0, 0]),
    rotation_axis=False,
    move_target=robot_model.link_joint2)
# array([-0.3183119 ,  0.63661397,  0.        ], dtype=float32)
viewer.redraw()
print(robot_model.link_joint2.copy_worldcoords())
#<Coordinates 0x176d61c50 1.900 0.000 0.000 / 0.0 0.3 0.0>

Fetch mesh model loading is slow

When I visualize fetch's mesh model, it takes about 17 seconds.
I think this is very slow...

import skrobot
robot_model = skrobot.models.urdf.RobotModelFromURDF(urdf_file=skrobot.data.fetch_urdfpath())
viewer = skrobot.viewers.TrimeshSceneViewer(resolution=(640, 480))
viewer.add(robot_model)
viewer.show()

With the following urdf, loading fetch's mesh model takes about 1 second.
To reduce the file size, several obj files in the original urdf is replaced with collision stl files.
https://gist.github.com/708yamaguchi/0558c3dcbc98dae2e88f40a8c43ebb77

I think some obj file size in the original urdf should be reduced.
How can I do?

Directory Structure

Proposal

  • skrobot
    • model.py
    • models/
      • fetch.py
      • pr2.py
      • kuka.py
    • interface.py
    • interfaces/
      • _pybullet.py
      • _pyrep.py
    • coordinate.py
    • coordinates/
      • quaternion.py
      • dual_quaternion.py
      • math.py
      • symbol_math.py
      • geo.py
    • optimizer.py
      • optimizers/
        • ...
    • calibration
      • hand_eye/
        • dual_quaternion.py
    • visualization/
      • _trimesh.py
    • utils/
      • ...

skrobot's urdf cannot parse urdf becaue np.float is expired

Numpy expired np.float from version 1.24.0. (released 12/19) Thus, when scikit robot's core module urdf starts to fail when parsing urdf file.
See, Expired deprecations section in https://github.com/numpy/numpy/releases/tag/v1.24.0

In [2]: PR2()
---------------------------------------------------------------------------
AttributeError                            Traceback (most recent call last)
Input In [2], in <cell line: 1>()
----> 1 PR2()

File ~/python/scikit-robot/skrobot/models/pr2.py:18, in PR2.__init__(self, use_tight_joint_limit)
     17 def __init__(self, use_tight_joint_limit=True):
---> 18     super(PR2, self).__init__()
     20     self.rarm_end_coords = CascadedCoords(
     21         parent=self.r_gripper_tool_frame,
     22         name='rarm_end_coords')
     23     self.larm_end_coords = CascadedCoords(
     24         parent=self.l_gripper_tool_frame,
     25         name='larm_end_coords')

File ~/python/scikit-robot/skrobot/models/urdf.py:18, in RobotModelFromURDF.__init__(self, urdf, urdf_file)
     16     self.load_urdf_file(file_obj=urdf_file)
     17 else:
---> 18     self.load_urdf_file(file_obj=self.default_urdf_path)

File ~/python/scikit-robot/skrobot/model/robot_model.py:1734, in RobotModel.load_urdf_file(self, file_obj)
   1732 else:
   1733     self.urdf_path = getattr(file_obj, 'name', None)
-> 1734 self.urdf_robot_model = URDF.load(file_obj=file_obj)
   1735 root_link = self.urdf_robot_model.base_link
   1737 links = []

File ~/python/scikit-robot/skrobot/utils/urdf.py:3092, in URDF.load(file_obj)
   3089     path, _ = os.path.split(file_obj.name)
   3091 node = tree.getroot()
-> 3092 return URDF._from_xml(node, path)

File ~/python/scikit-robot/skrobot/utils/urdf.py:3201, in URDF._from_xml(cls, node, path)
   3198 @classmethod
   3199 def _from_xml(cls, node, path):
   3200     valid_tags = set(['joint', 'link', 'transmission', 'material'])
-> 3201     kwargs = cls._parse(node, path)
   3203     extra_xml_node = ET.Element('extra')
   3204     for child in node:

File ~/python/scikit-robot/skrobot/utils/urdf.py:365, in URDFType._parse(cls, node, path)
    345 """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS
    346 
    347 Parse all elements and attributes in the _ELEMENTS and _ATTRIBS
   (...)
    362     and elements in the class arrays.
    363 """
    364 kwargs = cls._parse_simple_attribs(node)
--> 365 kwargs.update(cls._parse_simple_elements(node, path))
    366 return kwargs

File ~/python/scikit-robot/skrobot/utils/urdf.py:339, in URDFType._parse_simple_elements(cls, node, path)
    332         if len(vs) == 0 and r:
    333             raise ValueError(
    334                 'Missing required subelement(s) of type {} when '
    335                 'parsing an object of type {}'.format(
    336                     t.__name__, cls.__name__
    337                 )
    338             )
--> 339         v = [t._from_xml(n, path) for n in vs]
    340     kwargs[a] = v
    341 return kwargs

File ~/python/scikit-robot/skrobot/utils/urdf.py:339, in <listcomp>(.0)
    332         if len(vs) == 0 and r:
    333             raise ValueError(
    334                 'Missing required subelement(s) of type {} when '
    335                 'parsing an object of type {}'.format(
    336                     t.__name__, cls.__name__
    337                 )
    338             )
--> 339         v = [t._from_xml(n, path) for n in vs]
    340     kwargs[a] = v
    341 return kwargs

File ~/python/scikit-robot/skrobot/utils/urdf.py:385, in URDFType._from_xml(cls, node, path)
    368 @classmethod
    369 def _from_xml(cls, node, path):
    370     """Create an instance of this class from an XML node.
    371 
    372     Parameters
   (...)
    383         An instance of this class parsed from the node.
    384     """
--> 385     return cls(**cls._parse(node, path))

File ~/python/scikit-robot/skrobot/utils/urdf.py:365, in URDFType._parse(cls, node, path)
    345 """Parse all elements and attributes in the _ELEMENTS and _ATTRIBS
    346 
    347 Parse all elements and attributes in the _ELEMENTS and _ATTRIBS
   (...)
    362     and elements in the class arrays.
    363 """
    364 kwargs = cls._parse_simple_attribs(node)
--> 365 kwargs.update(cls._parse_simple_elements(node, path))
    366 return kwargs

File ~/python/scikit-robot/skrobot/utils/urdf.py:329, in URDFType._parse_simple_elements(cls, node, path)
    327     v = node.find(t._TAG)
    328     if r or v is not None:
--> 329         v = t._from_xml(v, path)
    330 else:
    331     vs = node.findall(t._TAG)

File ~/python/scikit-robot/skrobot/utils/urdf.py:1282, in Inertial._from_xml(cls, node, path)
   1276 zz = float(n.attrib['izz'])
   1277 inertia = np.array([
   1278     [xx, xy, xz],
   1279     [xy, yy, yz],
   1280     [xz, yz, zz]
   1281 ])
-> 1282 return Inertial(mass=mass, inertia=inertia, origin=origin)

File ~/python/scikit-robot/skrobot/utils/urdf.py:1227, in Inertial.__init__(self, mass, inertia, origin)
   1225 def __init__(self, mass, inertia, origin=None):
   1226     self.mass = mass
-> 1227     self.inertia = inertia
   1228     self.origin = origin

File ~/python/scikit-robot/skrobot/utils/urdf.py:1250, in Inertial.inertia(self, value)
   1248 @inertia.setter
   1249 def inertia(self, value):
-> 1250     value = np.asanyarray(value).astype(np.float)
   1251     if not np.allclose(value, value.T):
   1252         raise ValueError('Inertia must be a symmetric matrix')

File ~/.local/lib/python3.8/site-packages/numpy/__init__.py:284, in __getattr__(attr)
    281     from .testing import Tester
    282     return Tester
--> 284 raise AttributeError("module {!r} has no attribute "
    285                      "{!r}".format(__name__, attr))

AttributeError: module 'numpy' has no attribute 'float'

Code quality advise

Hi, I found a potential problem may be caused in the future (It's worked now.)
In model.py, line 957, function calc_inverse_kinematics_nspace_from_link_list, you set a default value (None) for paramter weight.
Then, it will be passed to function calc_nspace_from_joint_limit defined in line 943. And it will cause bug in line 950 if weight is None.

how to add child coordinate relative to the parent coordinate

I think one way to add a new child coords b to a is like below.

from skrobot.coordinates import make_cascoords
parent = make_cascoords(pos=[1, 0, 0])
child = make_cascoords(pos=[1, 1, 0])
parent.assoc(child)

Do you know some way to do the same thing w.r.t. parent? I mean, instead of globally define two coordinate and then connect them, it is more easier to define a child coordinate locally in the parent frame. My attempt is like below:

a = make_cascoords(pos=[1, 0, 0])
tmp = a.copy_worldcoords()
b = CascadedCoords(pos=tmp.worldpos(), rot=tmp.worldrot())
b.translate([0, 1, 0])
a.assoc(b)

but I believe there are some other nicer way.

Kuka yaml load warnings

% python kuka_example.py
pybullet build time: Apr  2 2019 00:37:57
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Unknown attribute: name
Version = 4.1 INTEL-12.4.7
Vendor = Intel Inc.
Renderer = Intel(R) Iris(TM) Graphics 650
b3Printf: Selected demo: Physics Server
startThreads creating 1 threads.
starting thread 0
started thread 0
MotionThreadFunc thread started

Any idea how to fix Unknown attribute: name?

refactoring around model directory makes examples not working

after this #174,
example/signed_distance_function.py
example/trimesh_scene_viewer.py
example/pybullet_robot_interface.py
are no longer be working, because they all import primitive from models.primitives.

I think maybe running all examples in the github action to check them working is a good practice. But also this makes test slower a lot.

PR2 init error

% pytest -vs tests/skrobot_tests/test_robots.py -k PR2
wkentaro at indigo in ~/scikit-robot tm 06:15 on master workon conda:base (~/scikit-robot/.anaconda3)
% pytest -vs tests/skrobot_tests/test_robots.py -k PR2
================================================ test session starts =================================================
platform darwin -- Python 3.6.8, pytest-4.4.0, py-1.8.0, pluggy-0.9.0 -- /Users/wkentaro/scikit-robot/.anaconda3/bin/python
cachedir: .pytest_cache
rootdir: /Users/wkentaro/Documents/scikit-robot
collected 3 items / 2 deselected / 1 selected

tests/skrobot_tests/test_robots.py::TestPR2::test_init FAILED

====================================================== FAILURES ======================================================
_________________________________________________ TestPR2.test_init __________________________________________________

self = <skrobot_tests.test_robots.TestPR2 testMethod=test_init>

    def test_init(self):
>       skrobot.robot_models.PR2()

tests/skrobot_tests/test_robots.py:9:
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
skrobot/robot_models/pr2.py:37: in __init__
    self.load_urdf(self.urdf_path)
skrobot/robot_model.py:1896: in load_urdf
    self.urdf_robot_model = URDF.load(str(urdf_path))
skrobot/utils/urdf.py:3066: in load
    return URDF._from_xml(node, path)
skrobot/utils/urdf.py:3175: in _from_xml
    kwargs = cls._parse(node, path)
skrobot/utils/urdf.py:340: in _parse
    kwargs.update(cls._parse_simple_elements(node, path))
skrobot/utils/urdf.py:314: in _parse_simple_elements
    v = [t._from_xml(n, path) for n in vs]
skrobot/utils/urdf.py:314: in <listcomp>
    v = [t._from_xml(n, path) for n in vs]
skrobot/utils/urdf.py:360: in _from_xml
    return cls(**cls._parse(node, path))
skrobot/utils/urdf.py:340: in _parse
    kwargs.update(cls._parse_simple_elements(node, path))
skrobot/utils/urdf.py:314: in _parse_simple_elements
    v = [t._from_xml(n, path) for n in vs]
skrobot/utils/urdf.py:314: in <listcomp>
    v = [t._from_xml(n, path) for n in vs]
skrobot/utils/urdf.py:1175: in _from_xml
    kwargs = cls._parse(node, path)
skrobot/utils/urdf.py:340: in _parse
    kwargs.update(cls._parse_simple_elements(node, path))
skrobot/utils/urdf.py:304: in _parse_simple_elements
    v = t._from_xml(v, path)
skrobot/utils/urdf.py:360: in _from_xml
    return cls(**cls._parse(node, path))
skrobot/utils/urdf.py:340: in _parse
    kwargs.update(cls._parse_simple_elements(node, path))
skrobot/utils/urdf.py:304: in _parse_simple_elements
    v = t._from_xml(v, path)
skrobot/utils/urdf.py:706: in _from_xml
    meshes = load_meshes(fn)
skrobot/utils/urdf.py:139: in load_meshes
    meshes = trimesh.load(filename)
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _

file_obj = <_io.BufferedReader name='/Users/wkentaro/.skrobot/pr2_description/meshes/base_v0/base.dae'>
file_type = 'dae', resolver = <trimesh.visual.resolvers.FilePathResolver object at 0x129369208>, kwargs = {}
out_types = ('Trimesh', 'Path', 'Scene')
metadata = {'file_name': 'base.dae', 'file_path': '/Users/wkentaro/.skrobot/pr2_description/meshes/base_v0/base.dae'}
opened = True

    def load(file_obj,
             file_type=None,
             resolver=None,
             **kwargs):
        """
        Load a mesh or vectorized path into objects:
        Trimesh, Path2D, Path3D, Scene

        Parameters
        ---------
        file_obj : str, or file- like object
          The source of the data to be loadeded
        file_type: str
          What kind of file type do we have (eg: 'stl')
        resolver : trimesh.visual.Resolver
          Object to load referenced assets like materials and textures
        kwargs : **
          Passed to geometry __init__

        Returns
        ---------
        geometry : Trimesh, Path2D, Path3D, Scene
          Loaded geometry as trimesh classes
        """
        # check to see if we're trying to load something
        # that is already a native trimesh object
        # do the check by name to avoid circular imports
        out_types = ('Trimesh', 'Path', 'Scene')
        if any(util.is_instance_named(file_obj, t)
               for t in out_types):
            log.info('Loaded called on %s object, returning input',
                     file_obj.__class__.__name__)
            return file_obj

        # parse the file arguments into clean loadable form
        (file_obj,  # file- like object
         file_type,  # str, what kind of file
         metadata,  # dict, any metadata from file name
         opened,    # bool, did we open the file ourselves
         resolver   # object to load referenced resources
         ) = parse_file_args(file_obj=file_obj,
                             file_type=file_type,
                             resolver=resolver)

        try:
            if isinstance(file_obj, dict):
                # if we've been passed a dict treat it as kwargs
                kwargs.update(file_obj)
                loaded = load_kwargs(kwargs)
            elif file_type in path_formats():
                # path formats get loaded with path loader
                loaded = load_path(file_obj,
                                   file_type=file_type,
                                   **kwargs)
            elif file_type in mesh_loaders:
                # mesh loaders use mesh loader
                loaded = load_mesh(file_obj,
                                   file_type=file_type,
                                   resolver=resolver,
                                   **kwargs)
            elif file_type in compressed_loaders:
                # for archives, like ZIP files
                loaded = load_compressed(file_obj,
                                         file_type=file_type,
                                         **kwargs)
            else:
                if file_type in ['svg', 'dxf']:
                    # call the dummy function to raise the import error
                    # this prevents the exception from being super opaque
                    load_path()
                else:
                    raise ValueError('File type: %s not supported' %
>                                    file_type)
E                                    ValueError: File type: dae not supported

../../scikit-robot/.anaconda3/lib/python3.6/site-packages/trimesh/exchange/load.py:148: ValueError

flake8 errors

With hacking (https://github.com/iory/scikit-robot/blob/master/.travis.yml#L9), there are lots of errors like below, and actually flake8 is not tested in Travis CI. What do you think of this? @iory

skrobot/__init__.py:7:1: E402 module level import not at top of file
skrobot/__init__.py:7:1: F401 'skrobot.coordinates' imported but unused
skrobot/__init__.py:17:1: H306  imports not in alphabetical order (skrobot.utils, skrobot.pybullet_robot_interface.pybulletrobotinterface)
skrobot/__init__.py:19:1: H306  imports not in alphabetical order (skrobot.utils.urdf.urdf, skrobot.quaternion)
skrobot/__init__.py:20:1: H306  imports not in alphabetical order (skrobot.quaternion, skrobot.data)
skrobot/coordinates.py:17:1: F401 'skrobot.math.rotation_matrix_from_rpy' imported but unused
skrobot/coordinates.py:99:1: H405  multi line docstring summary not separated with an empty line
skrobot/coordinates.py:117:1: H405  multi line docstring summary not separated with an empty line
skrobot/coordinates.py:143:1: H405  multi line docstring summary not separated with an empty line
skrobot/coordinates.py:143:45: H403  multi line docstrings should end on a new line
skrobot/coordinates.py:148:80: E501 line too long (85 > 79 characters)
skrobot/coordinates.py:153:1: H405  multi line docstring summary not separated with an empty line
skrobot/coordinates.py:153:32: H403  multi line docstrings should end on a new line
skrobot/coordinates.py:235:80: E501 line too long (112 > 79 characters)
skrobot/coordinates.py:244:1: H405  multi line docstring summary not separated with an empty line
skrobot/coordinates.py:244:51: H403  multi line docstrings should end on a new line
skrobot/coordinates.py:285:39: E711 comparison to None should be 'if cond is None:'
skrobot/coordinates.py:322:1: H405  multi line docstring summary not separated with an empty line
skrobot/coordinates.py:347:1: H404  multi line docstring should start without a leading new line
skrobot/coordinates.py:347:1: H405  multi line docstring summary not separated with an empty line
skrobot/coordinates.py:360:9: F841 local variable 'rot' is assigned to but never used
skrobot/coordinates.py:482:1: H404  multi line docstring should start without a leading new line
skrobot/coordinates.py:482:1: H405  multi line docstring summary not separated with an empty line
skrobot/dual_quaternion.py:31:1: H404  multi line docstring should start without a leading new line
skrobot/dual_quaternion.py:31:1: H405  multi line docstring summary not separated with an empty line
skrobot/dual_quaternion.py:43:1: H404  multi line docstring should start without a leading new line
skrobot/dual_quaternion.py:43:1: H405  multi line docstring summary not separated with an empty line
skrobot/dual_quaternion.py:93:1: H404  multi line docstring should start without a leading new line
skrobot/dual_quaternion.py:93:1: H405  multi line docstring summary not separated with an empty line
skrobot/dual_quaternion.py:110:1: H404  multi line docstring should start without a leading new line
skrobot/dual_quaternion.py:110:1: H405  multi line docstring summary not separated with an empty line
skrobot/dual_quaternion.py:155:1: H404  multi line docstring should start without a leading new line
skrobot/dual_quaternion.py:167:1: H404  multi line docstring should start without a leading new line
skrobot/dual_quaternion.py:167:1: H405  multi line docstring summary not separated with an empty line
skrobot/dual_quaternion.py:203:1: H404  multi line docstring should start without a leading new line
skrobot/geo.py:23:80: E501 line too long (81 > 79 characters)
skrobot/geo.py:37:1: H404  multi line docstring should start without a leading new line
skrobot/interpolator.py:22:80: E501 line too long (111 > 79 characters)
skrobot/interpolator.py:111:1: H404  multi line docstring should start without a leading new line
skrobot/interpolator.py:115:80: E501 line too long (90 > 79 characters)
skrobot/interpolator.py:120:80: E501 line too long (90 > 79 characters)
skrobot/math.py:60:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:62:80: E501 line too long (92 > 79 characters)
skrobot/math.py:65:80: E501 line too long (84 > 79 characters)
skrobot/math.py:75:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:76:80: E501 line too long (98 > 79 characters)
skrobot/math.py:83:80: E501 line too long (87 > 79 characters)
skrobot/math.py:98:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:98:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:116:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:116:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:137:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:174:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:200:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:200:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:218:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:218:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:256:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:256:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:276:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:276:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:381:80: E501 line too long (91 > 79 characters)
skrobot/math.py:411:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:411:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:437:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:437:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:457:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:457:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:501:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:501:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:530:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:556:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:556:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:584:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:584:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:608:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:608:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:648:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:648:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:688:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:688:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:712:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:730:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:750:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:772:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:772:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:794:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:794:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:820:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:820:1: H405  multi line docstring summary not separated with an empty line
skrobot/math.py:860:1: H404  multi line docstring should start without a leading new line
skrobot/math.py:860:1: H405  multi line docstring summary not separated with an empty line
skrobot/optimizer.py:43:1: H405  multi line docstring summary not separated with an empty line
skrobot/pybullet_robot_interface.py:6:1: H306  imports not in alphabetical order (skrobot.math.quaternion2rpy, skrobot.math.matrix2quaternion)
skrobot/pybullet_robot_interface.py:90:80: E501 line too long (85 > 79 characters)
skrobot/pybullet_robot_interface.py:98:80: E501 line too long (85 > 79 characters)
skrobot/pybullet_robot_interface.py:226:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:2:1: H306  imports not in alphabetical order (logging.getlogger, datetime)
skrobot/robot_interface.py:4:1: H306  imports not in alphabetical order (sys, numbers.number)
skrobot/robot_interface.py:10:1: H306  imports not in alphabetical order (rospy, actionlib)
skrobot/robot_interface.py:12:1: H306  imports not in alphabetical order (sensor_msgs.msg.jointstate, control_msgs.msg)
skrobot/robot_interface.py:26:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:26:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:52:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:52:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:63:80: E501 line too long (100 > 79 characters)
skrobot/robot_interface.py:64:80: E501 line too long (80 > 79 characters)
skrobot/robot_interface.py:68:80: E501 line too long (106 > 79 characters)
skrobot/robot_interface.py:73:80: E501 line too long (106 > 79 characters)
skrobot/robot_interface.py:100:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:100:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:128:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:128:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:201:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:219:80: E501 line too long (100 > 79 characters)
skrobot/robot_interface.py:221:80: E501 line too long (89 > 79 characters)
skrobot/robot_interface.py:249:80: E501 line too long (92 > 79 characters)
skrobot/robot_interface.py:252:80: E501 line too long (82 > 79 characters)
skrobot/robot_interface.py:310:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:310:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:365:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:365:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:368:80: E501 line too long (80 > 79 characters)
skrobot/robot_interface.py:379:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:379:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:446:80: E501 line too long (104 > 79 characters)
skrobot/robot_interface.py:447:80: E501 line too long (82 > 79 characters)
skrobot/robot_interface.py:449:80: E501 line too long (81 > 79 characters)
skrobot/robot_interface.py:455:80: E501 line too long (81 > 79 characters)
skrobot/robot_interface.py:462:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:462:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:471:9: F841 local variable 'traj_points' is assigned to but never used
skrobot/robot_interface.py:472:9: F841 local variable 'st' is assigned to but never used
skrobot/robot_interface.py:473:9: F841 local variable 'av_prev' is assigned to but never used
skrobot/robot_interface.py:490:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:490:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:501:11: H101  Use TODO(NAME)
skrobot/robot_interface.py:523:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:523:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:536:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:536:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interface.py:537:20: F821 undefined name 'reduce'
skrobot/robot_interface.py:565:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interface.py:565:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_model.py:2:1: H306  imports not in alphabetical order (logging.getlogger, itertools)
skrobot/robot_model.py:17:1: H306  imports not in alphabetical order (skrobot.utils.urdf.urdf, skrobot.geo.midcoords)
skrobot/robot_model.py:315:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_model.py:315:52: H403  multi line docstrings should end on a new line
skrobot/robot_model.py:415:80: E501 line too long (82 > 79 characters)
skrobot/robot_model.py:429:80: E501 line too long (85 > 79 characters)
skrobot/robot_model.py:431:80: E501 line too long (85 > 79 characters)
skrobot/robot_model.py:434:80: E501 line too long (113 > 79 characters)
skrobot/robot_model.py:440:80: E501 line too long (86 > 79 characters)
skrobot/robot_model.py:442:80: E501 line too long (86 > 79 characters)
skrobot/robot_model.py:444:80: E501 line too long (93 > 79 characters)
skrobot/robot_model.py:446:80: E501 line too long (93 > 79 characters)
skrobot/robot_model.py:455:80: E501 line too long (87 > 79 characters)
skrobot/robot_model.py:457:80: E501 line too long (96 > 79 characters)
skrobot/robot_model.py:458:80: E501 line too long (87 > 79 characters)
skrobot/robot_model.py:460:80: E501 line too long (96 > 79 characters)
skrobot/robot_model.py:581:80: E501 line too long (81 > 79 characters)
skrobot/robot_model.py:601:80: E501 line too long (91 > 79 characters)
skrobot/robot_model.py:602:80: E501 line too long (93 > 79 characters)
skrobot/robot_model.py:603:80: E501 line too long (95 > 79 characters)
skrobot/robot_model.py:605:80: E501 line too long (101 > 79 characters)
skrobot/robot_model.py:607:80: E501 line too long (84 > 79 characters)
skrobot/robot_model.py:679:1: H404  multi line docstring should start without a leading new line
skrobot/robot_model.py:745:80: E501 line too long (84 > 79 characters)
skrobot/robot_model.py:802:80: E501 line too long (82 > 79 characters)
skrobot/robot_model.py:816:1: H404  multi line docstring should start without a leading new line
skrobot/robot_model.py:828:80: E501 line too long (108 > 79 characters)
skrobot/robot_model.py:829:80: E501 line too long (80 > 79 characters)
skrobot/robot_model.py:831:80: E501 line too long (88 > 79 characters)
skrobot/robot_model.py:947:80: E501 line too long (81 > 79 characters)
skrobot/robot_model.py:961:80: E501 line too long (80 > 79 characters)
skrobot/robot_model.py:1025:80: E501 line too long (80 > 79 characters)
skrobot/robot_model.py:1034:15: H101  Use TODO(NAME)
skrobot/robot_model.py:1165:1: H404  multi line docstring should start without a leading new line
skrobot/robot_model.py:1173:80: E501 line too long (103 > 79 characters)
skrobot/robot_model.py:1189:80: E501 line too long (85 > 79 characters)
skrobot/robot_model.py:1251:80: E501 line too long (90 > 79 characters)
skrobot/robot_model.py:1252:80: E501 line too long (104 > 79 characters)
skrobot/robot_model.py:1268:80: E501 line too long (108 > 79 characters)
skrobot/robot_model.py:1270:80: E501 line too long (96 > 79 characters)
skrobot/robot_model.py:1279:80: E501 line too long (84 > 79 characters)
skrobot/robot_model.py:1283:80: E501 line too long (94 > 79 characters)
skrobot/robot_model.py:1284:80: E501 line too long (90 > 79 characters)
skrobot/robot_model.py:1392:15: H101  Use TODO(NAME)
skrobot/robot_model.py:1397:11: H101  Use TODO(NAME)
skrobot/robot_model.py:1442:1: H404  multi line docstring should start without a leading new line
skrobot/robot_model.py:1442:1: H405  multi line docstring summary not separated with an empty line
skrobot/symbol_math.py:8:1: H306  imports not in alphabetical order (sympy.s, sympy.eye)
skrobot/symbol_math.py:43:1: H404  multi line docstring should start without a leading new line
skrobot/symbol_math.py:43:1: H405  multi line docstring summary not separated with an empty line
skrobot/symbol_math.py:106:80: E501 line too long (81 > 79 characters)
skrobot/symbol_math.py:131:1: H404  multi line docstring should start without a leading new line
skrobot/symbol_math.py:131:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interfaces/pr2_interface.py:2:1: H306  imports not in alphabetical order (rospy, control_msgs.msg)
skrobot/robot_interfaces/pr2_interface.py:3:1: F401 'pr2_controllers_msgs.msg' imported but unused
skrobot/robot_interfaces/pr2_interface.py:12:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interfaces/pr2_interface.py:12:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interfaces/pr2_interface.py:52:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interfaces/pr2_interface.py:52:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_interfaces/pr2_interface.py:64:11: H101  Use TODO(NAME)
skrobot/robot_interfaces/pr2_interface.py:118:1: H404  multi line docstring should start without a leading new line
skrobot/robot_interfaces/pr2_interface.py:118:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/basics.py:1:1: F401 'string' imported but unused
skrobot/utils/basics.py:3:1: H306  imports not in alphabetical order (yaml, collections)
skrobot/utils/basics.py:8:1: F401 'xml.etree.ElementTree.ElementTree' imported but unused
skrobot/utils/basics.py:55:1: H401  docstring should not start with a space
skrobot/utils/core.py:2:1: H306  imports not in alphabetical order (logging.getlogger, copy)
skrobot/utils/core.py:4:1: F403 'from skrobot.utils.basics import *' used; unable to detect undefined names
skrobot/utils/core.py:4:34: H303  No wildcard (*) import.
skrobot/utils/core.py:21:1: H404  multi line docstring should start without a leading new line
skrobot/utils/core.py:21:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/core.py:32:1: H401  docstring should not start with a space
skrobot/utils/core.py:48:1: H404  multi line docstring should start without a leading new line
skrobot/utils/core.py:48:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/core.py:66:1: H401  docstring should not start with a space
skrobot/utils/core.py:106:1: H401  docstring should not start with a space
skrobot/utils/core.py:117:1: H404  multi line docstring should start without a leading new line
skrobot/utils/core.py:117:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/core.py:168:1: H404  multi line docstring should start without a leading new line
skrobot/utils/core.py:168:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/core.py:176:20: F405 xml_children may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/core.py:187:1: H404  multi line docstring should start without a leading new line
skrobot/utils/core.py:187:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/core.py:273:1: H401  docstring should not start with a space
skrobot/utils/core.py:304:1: H401  docstring should not start with a space
skrobot/utils/core.py:350:20: F405 node_add may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/core.py:370:1: H238  old style class declaration, use new style (inherit from `object`)
skrobot/utils/core.py:371:1: H401  docstring should not start with a space
skrobot/utils/core.py:375:25: F405 xml_children may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/core.py:385:1: H401  docstring should not start with a space
skrobot/utils/core.py:385:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/core.py:492:14: F405 YamlReflection may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/core.py:493:1: H401  docstring should not start with a space
skrobot/utils/core.py:504:1: H401  docstring should not start with a space
skrobot/utils/core.py:504:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/core.py:504:53: H403  multi line docstrings should end on a new line
skrobot/utils/core.py:508:1: H401  docstring should not start with a space
skrobot/utils/core.py:514:1: H401  docstring should not start with a space
skrobot/utils/core.py:517:15: F405 etree may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/core.py:522:16: F405 xml_string may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/core.py:539:16: F405 etree may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/core.py:557:1: H401  docstring should not start with a space
skrobot/utils/core.py:564:1: H401  docstring should not start with a space
skrobot/utils/core.py:564:1: H405  multi line docstring summary not separated with an empty line
skrobot/utils/core.py:564:71: H403  multi line docstrings should end on a new line
skrobot/utils/core.py:582:1: H401  docstring should not start with a space
skrobot/utils/core.py:591:16: F405 etree may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/type_check.py:13:1: H404  multi line docstring should start without a leading new line
skrobot/utils/type_check.py:35:1: H404  multi line docstring should start without a leading new line
skrobot/utils/urdf.py:1:1: F403 'from skrobot.utils.basics import *' used; unable to detect undefined names
skrobot/utils/urdf.py:1:34: H303  No wildcard (*) import.
skrobot/utils/urdf.py:138:20: F405 xml_children may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/urdf.py:144:17: F405 node_add may be undefined, or defined from star imports: skrobot.utils.basics
skrobot/utils/urdf.py:229:80: E501 line too long (89 > 79 characters)
skrobot/utils/urdf.py:420:1: H401  docstring should not start with a space
skrobot/utils/urdf.py:515:1: H404  multi line docstring should start without a leading new line
skrobot/utils/urdf.py:515:1: H405  multi line docstring summary not separated with an empty line
skrobot/optimizers/quadprog_solver.py:46:1: H404  multi line docstring should start without a leading new line
skrobot/optimizers/quadprog_solver.py:46:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_models/fetch.py:5:1: H306  imports not in alphabetical order (skrobot.robot_model.robotmodel, skrobot.coordinates.cascadedcoords)
skrobot/robot_models/fetch.py:14:1: H404  multi line docstring should start without a leading new line
skrobot/robot_models/fetch.py:14:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_models/kuka.py:6:1: H306  imports not in alphabetical order (skrobot.robot_model.robotmodel, skrobot.coordinates.cascadedcoords)
skrobot/robot_models/kuka.py:12:1: H404  multi line docstring should start without a leading new line
skrobot/robot_models/kuka.py:12:1: H405  multi line docstring summary not separated with an empty line
skrobot/robot_models/pr2.py:7:1: H404  multi line docstring should start without a leading new line
skrobot/robot_models/pr2.py:7:1: H405  multi line docstring summary not separated with an empty line

Initialization of UnionSDF from RobotModel failed when original URDF uses not 1 "scale" attribute.

When using a urdf like below.

<?xml version="1.0"?>
<robot name="myfirst">
  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="./bunny.stl" scale="10 10 10" />
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="./bunny.stl" scale="10 10 10" />
      </geometry>
    </collision>
  </link>
</robot>

Creation of UnionSDF fails like below.

In [5]: from skrobot.models.urdf import RobotModelFromURDF

In [6]: robot = RobotModelFromURDF(urdf_file=r"C:\Users\shinjo\Desktop\test_skrobot\test.urdf")

In [7]: from skrobot.sdf import UnionSDF

In [8]: sdf = UnionSDF.from_robot_model(robot)
---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
Cell In[8], line 1
----> 1 sdf = UnionSDF.from_robot_model(robot)

File d:\integral\dev\venv\lib\site-packages\skrobot\sdf\signed_distance_function.py:285, in UnionSDF.from_robot_model(cls, robot_model, dim_grid)
    283 for link in robot_model.link_list:
    284     if link.collision_mesh is not None:
--> 285         sdf = link2sdf(link, robot_model.urdf_path, dim_grid=dim_grid)
    286         sdf_list.append(sdf)
    287 return cls(sdf_list)

File d:\integral\dev\venv\lib\site-packages\skrobot\sdf\signed_distance_function.py:89, in link2sdf(link, urdf_path, dim_grid)
     69 def link2sdf(link, urdf_path, dim_grid=30):
     70     """Convert Link to corresponding sdf
     71
     72     Parameters
   (...)
     86         created.
     87     """
---> 89     sdf = trimesh2sdf(link.collision_mesh, dim_grid=dim_grid)
     90     link.assoc(sdf, relative_coords=sdf)
     91     return sdf

File d:\integral\dev\venv\lib\site-packages\skrobot\sdf\signed_distance_function.py:65, in trimesh2sdf(mesh, **gridsdf_kwargs)
     63     rotation_matrix = origin[:3, :3]
     64     translation = origin[:3, 3]
---> 65     sdf.newcoords(Coordinates(pos=translation, rot=rotation_matrix))
     66 return sdf

File d:\integral\dev\venv\lib\site-packages\skrobot\coordinates\base.py:209, in Coordinates.__init__(self, pos, rot, name, hook, check_validity)
    207     self._rotation = np.eye(3)
    208 else:
--> 209     self.rotation = rot
    210 if pos is None:
    211     self._translation = np.array([0, 0, 0])

File d:\integral\dev\venv\lib\site-packages\skrobot\coordinates\base.py:305, in Coordinates.rotation(self, rotation)
    302 if type(rotation) in (list, tuple):
    303     rotation = np.array(rotation).astype(np.float32)
--> 305 _check_valid_rotation(rotation)
    306 self._rotation = rotation * 1.

File d:\integral\dev\venv\lib\site-packages\skrobot\coordinates\math.py:120, in _check_valid_rotation(rotation)
    117     raise ValueError('Rotation must be specified as a 3x3 ndarray')
    119 if np.abs(np.linalg.det(rotation) - 1.0) > 1e-3:
--> 120     raise ValueError('Illegal rotation. Must have determinant == 1.0, '
    121                      'get {}'.format(np.linalg.det(rotation)))
    122 return rotation

ValueError: Illegal rotation. Must have determinant == 1.0, get 1000.0000000000007

rotation w.r.t. world should also change translation

I have question on the rotation of a coordinate. As the following snippets shows, the worldpos does not change after rotation wrt the world. Because in initialize step I set translation to [1, 0, 0] , I guess the world pos should be [0.0, 1.0, 0.0]. In the corresponding code, translation seems not be changed. Is this the correct behaviour?

import skrobot
import numpy as np
from math import *
from skrobot.coordinates import Coordinates

robot_model = skrobot.models.urdf.RobotModelFromURDF(urdf_file=skrobot.data.pr2_urdfpath())
def initialize():
    robot_model.translation = np.zeros(3)
    robot_model.rotation = np.eye(3)
    robot_model.translate([1.0, 0.0, 0.0])

initialize()
robot_model.rotate(pi*0.5, 'z', 'local')
print(robot_model.worldpos()) #[1.0, 0.0, 0.0] as I expected

initialize()
robot_model.rotate(pi*0.5, 'z', 'world')
print(robot_model.worldpos()) # this is also [1.0, 0.0, 0.0] but I think it's supposed to be [0, 1.0, 0]

Probelm about reset_manip_pose() for PR2

when I call reset_manip_pose() for PR2, torso link dosen't work right.

import skrobot
from skrobot.models import PR2
from skrobot.interfaces import PybulletRobotInterface

robot = PR2()
interface = PybulletRobotInterface(robot)

I added following code

interface.force = 10000

then it worked!

pyglet version=2.0.0 if install scikit-robot from pypi (local is ok though)

problem

after #259, pyglet==1.4.10 should be installed if python version <= 3.7.
As the ci results shows, there is no problem when installing from source. However, if install from pypi, somehow version specification is ignored, and latest version (==2.0.0) is installed. Because of this scikit-robot doesn't wok.

workadound

if you are python < 3.8 user don't use pypi.
Install from source (git clone and pip install .)

Interactive viewer through AttributionError

In case of pressing q, we have following error.

python interactive_viewer.py                                          
==> Waiting 3 seconds
==> Moving to reset_manip_pose
[  0.  10.   0. -90.   0.  90.   0.   0.   0.   0.   0.   0.]
==> Waiting 3 seconds
==> Moving to init_pose
[0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.]
==> Waiting 3 seconds
==> IK to box
[   9.759757     3.273693   -10.150737  -111.39293      0.8358345
   50.741776    89.24997      0.           0.           0.
    0.           0.       ]
==> Press [q] to close window

Exception in thread Thread-1:
Traceback (most recent call last):
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/threading.py", line 916, in _bootstrap_inner
    self.run()
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/threading.py", line 864, in run
    self._target(*self._args, **self._kwargs)
  File "/home/iory/src/github.com/develop/scikit-robot/examples/interactive_viewer.py", line 43, in _init_and_start_app
    super().__init__(*self._args, **self._kwargs)
  File "/home/iory/src/github.com/mikedh/trimesh/trimesh/viewer/windowed.py", line 149, in __init__
    pyglet.app.run()
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/app/__init__.py", line 142, in run
    event_loop.run()
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/app/base.py", line 175, in run
    self._run()
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/app/base.py", line 188, in _run
    platform_event_loop.step(timeout)
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/app/xlib.py", line 125, in step
    device.select()
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/canvas/xlib.py", line 201, in select
    dispatch(e)
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/window/xlib/__init__.py", line 897, in dispatch_platform_event_view
    event_handler(e)
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/window/xlib/__init__.py", line 1061, in _event_key_view
    self.dispatch_event('on_key_press', symbol, modifiers)
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/window/__init__.py", line 1313, in dispatch_event
    if EventDispatcher.dispatch_event(self, *args) != False:
  File "/home/iory/.pyenv/versions/anaconda3-5.3.0/envs/deeplearning/lib/python3.6/site-packages/pyglet/event.py", line 370, in dispatch_event
    if handler(*args):
  File "/home/iory/src/github.com/develop/scikit-robot/examples/interactive_viewer.py", line 94, in on_key_press
    return super().on_key_press(*args, **kwargs)
  File "/home/iory/src/github.com/mikedh/trimesh/trimesh/viewer/windowed.py", line 510, in on_key_press
    self.on_close()
  File "/home/iory/src/github.com/develop/scikit-robot/examples/interactive_viewer.py", line 101, in on_close
    self.thread.exit()
AttributeError: 'Thread' object has no attribute 'exit'

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    ๐Ÿ–– Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. ๐Ÿ“Š๐Ÿ“ˆ๐ŸŽ‰

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google โค๏ธ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.