Get contact points from contact plane

[AlexanderFabisch]

For each pair of colliders we only get the contact plane and depth with GJK+EPA or MPR (GJK+EPA: (closest_point, norm_vector(mtv)); MPR: (position, penetration_direction); indicated by red arrows in the above illustration). Find a way to get the contact shape in the contact plane.

connected to #32

Contact Points in Contact Plane

Overlapping Volumes

Combined Contact Normal

Code

"""
=====
Title
=====
"""
import numpy as np
import pytransform3d.visualizer as pv
import pytransform3d.rotations as pr
import pytransform3d.transformations as pt
from grasp_metrics.hands import MiaHand
from distance3d.broad_phase import BoundingVolumeHierarchy
from distance3d.colliders import Box, MeshGraph
from distance3d.mpr import mpr_penetration
from distance3d import containment_test
from distance3d.utils import plane_basis_from_normal, norm_vector
from distance3d.mesh import make_convex_mesh


hand = MiaHand()
joint_angles = hand.get_grasp_angles("lateral", 0.1)
for joint_name in joint_angles:
    hand.tm_.set_joint(joint_name, joint_angles[joint_name])

bvh = BoundingVolumeHierarchy(hand.tm_, hand.get_base_frame())
bvh.fill_tree_with_colliders(
    hand.tm_, make_artists=True, fill_self_collision_whitelists=True)

object_to_grasp = Box(pt.transform_from(R=np.eye(3), p=np.array([0.04, 0.14, 0.03])),
                      np.array([0.03, 0.025, 0.025]))
object_to_grasp.make_artist()
geometry = object_to_grasp.artist_.geometries[0]
aabb = geometry.get_axis_aligned_bounding_box()


def containment_test_mapping(points, collider):
    collider_type = collider.__class__.__name__
    if collider_type == "Cylinder":
        return containment_test.points_in_cylinder(points, collider.cylinder2origin, collider.radius, collider.length)
    elif collider_type == "Sphere":
        return containment_test.points_in_sphere(points, collider.c, collider.radius)
    elif collider_type == "Box":
        return containment_test.points_in_box(points, collider.box2origin, collider.size)
    elif collider_type == "MeshGraph":
        triangles = make_convex_mesh(collider.vertices)
        return containment_test.points_in_convex_mesh(points, collider.mesh2origin, collider.vertices, triangles)
    # TODO find a better way, attach to collider?
    raise NotImplementedError()


def mesh_volume(vertices, triangles):
    faces = vertices[triangles]
    A = faces[:, 1] - faces[:, 0]
    B = faces[:, 2] - faces[:, 0]
    triangle_normals = np.cross(A, B)

    com = np.mean(vertices, axis=0)
    vertices = vertices - com[np.newaxis]

    total_volume = 0.0
    for triangle_idx in range(len(triangles)):
        verts = vertices[triangles[triangle_idx]]
        triangle_normal = triangle_normals[triangle_idx]
        triangle_center = np.mean(verts, axis=0)
        normal_angle = pr.angle_between_vectors(
            triangle_center, triangle_normal)
        sign = -1.0 if normal_angle > np.pi / 2.0 else 1.0

        J = np.array([
            [verts[0, 0], verts[0, 1], verts[0, 2], 1.0],
            [verts[1, 0], verts[1, 1], verts[1, 2], 1.0],
            [verts[2, 0], verts[2, 1], verts[2, 2], 1.0],
            [0.0, 0.0, 0.0, 1.0],
        ])

        abs_det_J = np.linalg.det(J)

        volume = sign * abs_det_J / 6.0
        total_volume += volume
    return total_volume


fig = pv.figure()
for hand_collider in bvh.get_colliders():
    #hand_collider.artist_.add_artist(fig)
    geometry = hand_collider.artist_.geometries[0]
    #points = geometry.sample_points_poisson_disk(500)
    #fig.add_geometry(points)

contact_volumes = []
for hand_collider in bvh.get_colliders():
    intersection, depth, normal, contact_point = mpr_penetration(
        hand_collider, object_to_grasp)

    if intersection:
        # sample contact volume
        points = np.random.RandomState(5).randn(1000000, 3) * 0.03

        # if you only want to sample in contact plane:
        #points[:, 2] = 0.0

        x, y = plane_basis_from_normal(normal)
        R = np.column_stack((x, y, normal))
        points_in_world = points.dot(R.T) + contact_point

        contained_in_hand = containment_test_mapping(points_in_world, hand_collider)
        contained_in_object = containment_test_mapping(points_in_world, object_to_grasp)
        contained = np.logical_and(contained_in_hand, contained_in_object)
        contact_points = points_in_world[contained]
        if len(contact_points) == 0:
            continue

        # show overlapping mesh (always convex(?))
        triangles = make_convex_mesh(contact_points)
        convex_mesh = MeshGraph(np.eye(4), contact_points, triangles)
        convex_mesh.make_artist(c=(1, 0, 0))
        convex_mesh.artist_.add_artist(fig)

        #volume = mesh_volume(contact_points, triangles)
        contact_volumes.append((depth, normal))

        # show samples:
        #fig.scatter(contact_points, s=0.0005, c=(0, 0, 0))

        fig.plot_vector(contact_point, depth * normal, c=(1, 0, 0))

assert len(contact_volumes) > 0
weights = np.array([v[0] for v in contact_volumes])
weights /= sum(weights)
normals = np.array([v[1] for v in contact_volumes])
contact_normal = norm_vector(weights.dot(normals))
fig.plot_vector(object_to_grasp.center(), 0.01 * contact_normal, c=(0, 0, 1))

fig.add_geometry(aabb)
fig.show()