Skip to content

Commit

Permalink
spuport physx gpu sdf collision
Browse files Browse the repository at this point in the history
  • Loading branch information
fbxiang committed May 14, 2024
1 parent 7d834b9 commit 2b9cfbc
Show file tree
Hide file tree
Showing 16 changed files with 233 additions and 261 deletions.
4 changes: 2 additions & 2 deletions include/sapien/physx/collision_shape.h
Original file line number Diff line number Diff line change
Expand Up @@ -183,12 +183,12 @@ class PhysxCollisionShapeConvexMesh : public PhysxCollisionShape {
class PhysxCollisionShapeTriangleMesh : public PhysxCollisionShape {
public:
// TODO: support rotation?
PhysxCollisionShapeTriangleMesh(std::string const &filename, Vec3 const &scale,
PhysxCollisionShapeTriangleMesh(std::string const &filename, Vec3 const &scale, bool sdf,
std::shared_ptr<PhysxMaterial> material = nullptr);
PhysxCollisionShapeTriangleMesh(
Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor> const &vertices,
Eigen::Matrix<uint32_t, Eigen::Dynamic, 3, Eigen::RowMajor> const &triangles,
Vec3 const &scale, std::shared_ptr<PhysxMaterial> material = nullptr);
Vec3 const &scale, bool sdf, std::shared_ptr<PhysxMaterial> material = nullptr);

// internal use only
PhysxCollisionShapeTriangleMesh(std::shared_ptr<PhysxTriangleMesh> mesh, Vec3 const &scale,
Expand Down
8 changes: 4 additions & 4 deletions include/sapien/physx/mesh.h
Original file line number Diff line number Diff line change
Expand Up @@ -66,10 +66,10 @@ class PhysxConvexMesh {

class PhysxTriangleMesh {
public:
PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles);
PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles, bool generateSDF);
PhysxTriangleMesh(Vertices const &vertices, Triangles const &triangles,
std::string const &filename);
PhysxTriangleMesh(std::string const &filename);
std::string const &filename, bool generateSDF);
PhysxTriangleMesh(std::string const &filename, bool generateSDF);

::physx::PxTriangleMesh *getPxMesh() const { return mMesh; }
bool hasFilename() { return mFilename.has_value(); }
Expand All @@ -91,7 +91,7 @@ class PhysxTriangleMesh {
}

private:
void loadMesh(Vertices const &vertices, Triangles const &triangles);
void loadMesh(Vertices const &vertices, Triangles const &triangles, bool generateSDF);
std::shared_ptr<PhysxEngine> mEngine;
::physx::PxTriangleMesh *mMesh{};
std::optional<std::string> mFilename;
Expand Down
1 change: 1 addition & 0 deletions include/sapien/physx/mesh_manager.h
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ class MeshManager {
MeshManager();

std::shared_ptr<PhysxTriangleMesh> loadTriangleMesh(const std::string &filename);
std::shared_ptr<PhysxTriangleMesh> loadTriangleMeshWithSDF(const std::string &filename);
std::shared_ptr<PhysxConvexMesh> loadConvexMesh(const std::string &filename);
std::vector<std::shared_ptr<PhysxConvexMesh>> loadConvexMeshGroup(const std::string &filename);

Expand Down
17 changes: 14 additions & 3 deletions include/sapien/physx/physx_default.h
Original file line number Diff line number Diff line change
Expand Up @@ -25,16 +25,22 @@ struct PhysxSceneConfig {
};

struct PhysxBodyConfig {
uint32_t solverPositionIterations = 10; // solver position iterations, helps reduce jittering
uint32_t solverVelocityIterations = 1; // solver velocity iterations
float sleepThreshold = 0.005f; // put to sleep if (kinetic energy/(mass) falls below
uint32_t solverPositionIterations = 10; // solver position iterations, helps reduce jittering
uint32_t solverVelocityIterations = 1; // solver velocity iterations
float sleepThreshold = 0.005f; // put to sleep if (kinetic energy/(mass) falls below
};

struct PhysxShapeConfig {
float contactOffset = 0.01f; // how close should contacts be generated
float restOffset = 0.f;
};

struct PhysxSDFShapeConfig {
float spacing = 0.01f;
uint32_t subgridSize = 6;
uint32_t numThreadsForConstruction = 4;
};

class PhysxDefault {
public:
static std::shared_ptr<PhysxMaterial> GetDefaultMaterial();
Expand All @@ -61,6 +67,11 @@ class PhysxDefault {
static void setShapeConfig(PhysxShapeConfig const &);
static PhysxShapeConfig const &getShapeConfig();

static void setSDFShapeConfig(float spacing, uint32_t subgridSize,
uint32_t numThreadsForConstruction);
static void setSDFShapeConfig(PhysxSDFShapeConfig const &);
static PhysxSDFShapeConfig getSDFShapeConfig();

// enable GPU simulation, may not be disabled
static void EnableGPU();
static bool GetGPUEnabled();
Expand Down
21 changes: 21 additions & 0 deletions include/sapien/physx/physx_system.h
Original file line number Diff line number Diff line change
Expand Up @@ -168,6 +168,19 @@ class PhysxSystemGpu : public PhysxSystem {
CudaArrayHandle gpuGetRigidDynamicCudaHandle() const { return mCudaRigidDynamicHandle; }
CudaArrayHandle gpuGetArticulationLinkCudaHandle() const { return mCudaLinkHandle; }

CudaArrayHandle gpuGetRigidBodyForceCudaHandle() const {
return mCudaRigidBodyForceBuffer.handle();
}
CudaArrayHandle gpuGetRigidDynamicForceCudaHandle() const {
return mCudaRigidDynamicForceHandle;
}
CudaArrayHandle gpuGetRigidBodyTorqueCudaHandle() const {
return mCudaRigidBodyTorqueBuffer.handle();
}
CudaArrayHandle gpuGetRigidDynamicTorqueCudaHandle() const {
return mCudaRigidDynamicTorqueHandle;
}

CudaArrayHandle gpuGetArticulationQposCudaHandle() const { return mCudaQposHandle; }
CudaArrayHandle gpuGetArticulationQvelCudaHandle() const { return mCudaQvelHandle; }
CudaArrayHandle gpuGetArticulationQaccCudaHandle() const { return mCudaQaccHandle; }
Expand Down Expand Up @@ -195,6 +208,8 @@ class PhysxSystemGpu : public PhysxSystem {
void gpuApplyArticulationQTargetVel(CudaArrayHandle const &indices);

void gpuApplyRigidDynamicData();
void gpuApplyRigidDynamicForce();
void gpuApplyRigidDynamicTorque();
void gpuApplyArticulationRootPose();
void gpuApplyArticulationRootVel();
void gpuApplyArticulationQpos();
Expand Down Expand Up @@ -269,6 +284,12 @@ class PhysxSystemGpu : public PhysxSystem {
CudaArrayHandle mCudaRigidDynamicHandle;
CudaArrayHandle mCudaLinkHandle;

CudaArray mCudaRigidBodyForceBuffer;
CudaArrayHandle mCudaRigidDynamicForceHandle;

CudaArray mCudaRigidBodyTorqueBuffer;
CudaArrayHandle mCudaRigidDynamicTorqueHandle;

CudaHostArray mCudaHostRigidBodyBuffer;

CudaArray mCudaArticulationBuffer;
Expand Down
103 changes: 0 additions & 103 deletions manualtest/test.py

This file was deleted.

2 changes: 1 addition & 1 deletion python/py_package/__init__.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -34,5 +34,5 @@ from . import utils
from . import version
from . import wrapper
__all__ = ['ActorBuilder', 'ArticulationBuilder', 'Component', 'CudaArray', 'Device', 'Engine', 'Entity', 'Path', 'PinocchioModel', 'Pose', 'SapienRenderer', 'Scene', 'SceneConfig', 'System', 'Widget', 'asset', 'internal_renderer', 'math', 'os', 'physx', 'pkg_resources', 'platform', 'profile', 'pysapien', 'pysapien_pinocchio', 'render', 'set_log_level', 'simsense', 'utils', 'version', 'warn', 'wrapper']
__version__: str = '3.0.0.dev20240506+da1ab45a'
__version__: str = '3.0.0.dev20240514+4fcf746f'
__warningregistry__: dict = {'version': 0}
40 changes: 38 additions & 2 deletions python/py_package/pysapien/physx.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ import pybind11_stubgen.typing_ext
import sapien.pysapien
import sapien.pysapien_pinocchio
import typing
__all__ = ['PhysxArticulation', 'PhysxArticulationJoint', 'PhysxArticulationLinkComponent', 'PhysxBaseComponent', 'PhysxBodyConfig', 'PhysxCollisionShape', 'PhysxCollisionShapeBox', 'PhysxCollisionShapeCapsule', 'PhysxCollisionShapeConvexMesh', 'PhysxCollisionShapeCylinder', 'PhysxCollisionShapePlane', 'PhysxCollisionShapeSphere', 'PhysxCollisionShapeTriangleMesh', 'PhysxContact', 'PhysxContactPoint', 'PhysxCpuSystem', 'PhysxDistanceJointComponent', 'PhysxDriveComponent', 'PhysxEngine', 'PhysxGearComponent', 'PhysxGpuContactBodyImpulseQuery', 'PhysxGpuContactPairImpulseQuery', 'PhysxGpuSystem', 'PhysxJointComponent', 'PhysxMaterial', 'PhysxRayHit', 'PhysxRigidBaseComponent', 'PhysxRigidBodyComponent', 'PhysxRigidDynamicComponent', 'PhysxRigidStaticComponent', 'PhysxSceneConfig', 'PhysxShapeConfig', 'PhysxSystem', 'get_body_config', 'get_default_material', 'get_scene_config', 'get_shape_config', 'is_gpu_enabled', 'set_body_config', 'set_default_material', 'set_gpu_memory_config', 'set_scene_config', 'set_shape_config', 'version']
__all__ = ['PhysxArticulation', 'PhysxArticulationJoint', 'PhysxArticulationLinkComponent', 'PhysxBaseComponent', 'PhysxBodyConfig', 'PhysxCollisionShape', 'PhysxCollisionShapeBox', 'PhysxCollisionShapeCapsule', 'PhysxCollisionShapeConvexMesh', 'PhysxCollisionShapeCylinder', 'PhysxCollisionShapePlane', 'PhysxCollisionShapeSphere', 'PhysxCollisionShapeTriangleMesh', 'PhysxContact', 'PhysxContactPoint', 'PhysxCpuSystem', 'PhysxDistanceJointComponent', 'PhysxDriveComponent', 'PhysxEngine', 'PhysxGearComponent', 'PhysxGpuContactBodyImpulseQuery', 'PhysxGpuContactPairImpulseQuery', 'PhysxGpuSystem', 'PhysxJointComponent', 'PhysxMaterial', 'PhysxRayHit', 'PhysxRigidBaseComponent', 'PhysxRigidBodyComponent', 'PhysxRigidDynamicComponent', 'PhysxRigidStaticComponent', 'PhysxSDFConfig', 'PhysxSceneConfig', 'PhysxShapeConfig', 'PhysxSystem', 'get_body_config', 'get_default_material', 'get_scene_config', 'get_sdf_config', 'get_shape_config', 'is_gpu_enabled', 'set_body_config', 'set_default_material', 'set_gpu_memory_config', 'set_scene_config', 'set_sdf_config', 'set_shape_config', 'version']
M = typing.TypeVar("M", bound=int)
class PhysxArticulation:
name: str
Expand Down Expand Up @@ -424,7 +424,7 @@ class PhysxCollisionShapeSphere(PhysxCollisionShape):
def radius(self) -> float:
...
class PhysxCollisionShapeTriangleMesh(PhysxCollisionShape):
def __init__(self, filename: str, scale: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float32]] | list[float] | tuple, material: PhysxMaterial) -> None:
def __init__(self, filename: str, scale: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float32]] | list[float] | tuple, sdf: bool, material: PhysxMaterial) -> None:
...
def get_scale(self) -> numpy.ndarray[typing.Literal[3], numpy.dtype[numpy.float32]]:
...
Expand Down Expand Up @@ -630,6 +630,10 @@ class PhysxGpuSystem(PhysxSystem):
@typing.overload
def gpu_apply_rigid_dynamic_data(self, index_buffer: sapien.pysapien.CudaArray) -> None:
...
def gpu_apply_rigid_dynamic_force(self) -> None:
...
def gpu_apply_rigid_dynamic_torque(self) -> None:
...
def gpu_create_contact_body_impulse_query(self, bodies: list[PhysxRigidBaseComponent]) -> PhysxGpuContactBodyImpulseQuery:
...
def gpu_create_contact_pair_impulse_query(self, body_pairs: list[tuple[PhysxRigidBaseComponent, PhysxRigidBaseComponent]]) -> PhysxGpuContactPairImpulseQuery:
Expand Down Expand Up @@ -722,9 +726,21 @@ class PhysxGpuSystem(PhysxSystem):
def cuda_rigid_body_data(self) -> sapien.pysapien.CudaArray:
...
@property
def cuda_rigid_body_force(self) -> sapien.pysapien.CudaArray:
...
@property
def cuda_rigid_body_torque(self) -> sapien.pysapien.CudaArray:
...
@property
def cuda_rigid_dynamic_data(self) -> sapien.pysapien.CudaArray:
...
@property
def cuda_rigid_dynamic_force(self) -> sapien.pysapien.CudaArray:
...
@property
def cuda_rigid_dynamic_torque(self) -> sapien.pysapien.CudaArray:
...
@property
def device(self) -> sapien.pysapien.Device:
...
class PhysxJointComponent(PhysxBaseComponent):
Expand Down Expand Up @@ -936,6 +952,18 @@ class PhysxRigidDynamicComponent(PhysxRigidBodyComponent):
class PhysxRigidStaticComponent(PhysxRigidBaseComponent):
def __init__(self) -> None:
...
class PhysxSDFConfig:
num_threads_for_construction: int
spacing: float
subgridSize: int
def __getstate__(self) -> tuple:
...
def __init__(self) -> None:
...
def __repr__(self) -> str:
...
def __setstate__(self, arg0: tuple) -> None:
...
class PhysxSceneConfig:
bounce_threshold: float
enable_ccd: bool
Expand Down Expand Up @@ -1004,6 +1032,8 @@ def get_default_material() -> PhysxMaterial:
...
def get_scene_config() -> PhysxSceneConfig:
...
def get_sdf_config() -> PhysxSDFConfig:
...
def get_shape_config() -> PhysxShapeConfig:
...
def is_gpu_enabled() -> bool:
Expand All @@ -1025,6 +1055,12 @@ def set_scene_config(gravity: numpy.ndarray[typing.Literal[3], numpy.dtype[numpy
def set_scene_config(config: PhysxSceneConfig) -> None:
...
@typing.overload
def set_sdf_config(spacing: float = 0.009999999776482582, subgrid_size: int = 6, num_threads_for_construction: int = 4) -> None:
...
@typing.overload
def set_sdf_config(config: PhysxSDFConfig) -> None:
...
@typing.overload
def set_shape_config(contact_offset: float = 0.009999999776482582, rest_offset: float = 0.0) -> None:
...
@typing.overload
Expand Down
10 changes: 6 additions & 4 deletions python/py_package/wrapper/actor_builder.py
Original file line number Diff line number Diff line change
Expand Up @@ -72,9 +72,9 @@ class VisualShapeRecord:
radius: float = 1
length: float = 1

material: Union[
sapien.render.RenderMaterial, None
] = None # None is only used for mesh
material: Union[sapien.render.RenderMaterial, None] = (
None # None is only used for mesh
)

pose: sapien.Pose = sapien.Pose()
name: str = ""
Expand Down Expand Up @@ -219,6 +219,7 @@ def build_physx_component(self, link_parent=None):
shape = sapien.physx.PhysxCollisionShapeTriangleMesh(
filename=preprocess_mesh_file(r.filename),
scale=r.scale,
sdf=self.physx_body_type == "dynamic",
material=r.material,
)
shapes = [shape]
Expand Down Expand Up @@ -492,6 +493,7 @@ def add_nonconvex_collision_from_file(
pose: sapien.Pose = sapien.Pose(),
scale: Vec3 = (1, 1, 1),
material: Union[sapien.physx.PhysxMaterial, None] = None,
density: float = 1000,
patch_radius: float = 0,
min_patch_radius: float = 0,
is_trigger: bool = False,
Expand All @@ -506,7 +508,7 @@ def add_nonconvex_collision_from_file(
pose=pose,
scale=scale,
material=material,
density=0,
density=density,
patch_radius=patch_radius,
min_patch_radius=min_patch_radius,
is_trigger=is_trigger,
Expand Down
Loading

0 comments on commit 2b9cfbc

Please sign in to comment.