jhavl / spatialgeometry Goto Github PK

I just noticed that spatialgeometry installs scene.so directly into the site-packages directory, which makes it globally available under import scene.

I think it might be more fitting to place it under spatialgeometry? So that the import is from spatialgeometry import scene?

Description

I am receiving True from the iscollided function for robot positions that are nowhere close to the collision shape. At least according to what is visualized (Swift). Is there something I am doing wrong, or is this a bug?

It seems like the collision might be based on convex hull. If so, how would it be possible to change this?

Screenshots

Figures of part and robot positions


With the collision model visualized. This position is shifted even more to the side, but is still colliding apparently.

Link collision output

If collided, link object

pybullet build time: May 21 2022 18:07:31
False Link("panda_link0")
False Link("panda_link1", SE3(0, 0, 0.333) ⊕ Rz(q), parent="panda_link0", qlim=[-2.9, 2.9])
False Link("panda_link2", SE3(-90°, -0°, 0°) ⊕ Rz(q), parent="panda_link1", qlim=[-1.76, 1.76])
False Link("panda_link3", SE3(0, -0.316, 0; 90°, -0°, 0°) ⊕ Rz(q), parent="panda_link2", qlim=[-2.9, 2.9])
False Link("panda_link4", SE3(0.0825, 0, 0; 90°, -0°, 0°) ⊕ Rz(q), parent="panda_link3", qlim=[-3.07, -0.0698])
False Link("panda_link5", SE3(-0.0825, 0.384, 0; -90°, -0°, 0°) ⊕ Rz(q), parent="panda_link4", qlim=[-2.9, 2.9])
True Link("panda_link6", SE3(90°, -0°, 0°) ⊕ Rz(q), parent="panda_link5", qlim=[-0.0175, 3.75])
True Link("panda_link7", SE3(0.088, 0, 0; 90°, -0°, 0°) ⊕ Rz(q), parent="panda_link6", qlim=[-2.9, 2.9])
True Link("panda_link8", SE3(0, 0, 0.107), parent="panda_link7")

Version information

roboticstoolbox-python: 1.0.1
spatialgeometry: 1.0.1
spatialmath-python: 1.0.0
swift-sim: 1.0.0

Code

import roboticstoolbox as rtb
from spatialmath import SE3
from spatialgeometry import Mesh
from swift import Swift
import numpy as np

panda = rtb.models.URDF.Panda()

panda.base = SE3([0.7,-0.5,0])
panda.q = [0,1.5,0,0,0,2,0]

part_pos = np.array([
    [0.9553, 0, 0.2955, 1.349],
    [0, 1, 0, 0],
    [-0.2955, 0, 0.9553, -0.1874],
    [0, 0, 0, 1]
])

part = Mesh(
    filename="test_temp.stl",
    color=[34, 143, 201],
    scale=(0.001,) * 3,
    pose = SE3(part_pos, check=False)
    # pose = mt1.fkine(mt1.q)
)

env = Swift()
env.launch()
env.add(part)
env.add(panda)
# env.add(mt1)

for link in panda.links:
    print(link.iscollided(part), link)

Expected behavior

iscollided function to return False

Environment

Python: 3.9.6
Windows 10

Hello developers.
Thank you for this very useful package!

I've noticed that in your implementation of PyBullet, when computing the closest point between two bodies, you always return the first distance (array index 0). Yet, I can find any info (in the PyBullet Quick Start Guide) that tells us that this array is ordered. In fact, I've done a small test which shows that it isn't actually ordered. Am I misunderstanding something?

Dear,
I am using this project combined with robotictoolbox to achieve collision avoidance. First I need to create a obstacles objects, could you give some suggestions or code about how to visualize these objects?

obstacle = sg.Cuboid((length, width, height), pose = SE3(5, 0, 0))
obstacle.plot()????

Instead of doing pip.install() in the setup.py

Any chance you could include it so we can build on conda-forge @jhavl ?

Dear,
I am using this project combined with robotictoolbox to achieve collision avoidance. First I need to create a obstacles objects, could you give some suggestions or code about how to visualize these objects?

obstacle = sg.Cuboid((length, width, height), pose = SE3(5, 0, 0))
obstacle.plot()????