def find_contact_center(finger_poly,normal,distal,surface,d,z,obj_l):
"""
Given the finger polygon, corresponding vectors, finger and object parameters, determines the pivoting center for the finger
Parameters
----------
finger_poly: Geometry3D.ConvexPolygon
Finger polygon
normal: Geometry3D.Vector
Finger normal
distal: Geometry3D.Vector
Finger distal vector
surface: Geometry3D.ConvexPolygon
Polygon corresponding to the contact surface
d: float
Finger parameter d
z: float
Finger parameter z
obj_l: float
Object length along distal direction
Returns
----------
center: Geometry3D.Point
Pivoting center
"""
# Check if the object exceeds the tip of the finger
if finger_w - d - obj_l > 0:
# if not no translation is necessary along distal
v1 = distal*0
else:
v1 = (-(finger_w-d)/2 + (obj_l/2))*distal
# Determine the translation along the hand normal
v_temp = Vector(surface.center_point,finger_poly.center_point)
hand_normal = distal.cross(normal)
if v_temp.length() == 0:
b = 0
else:
ang = v_temp.angle(hand_normal)
if ang<np.pi/2:
b = v_temp.length()*abs(np.cos(ang))
else:
b = -v_temp.length()*abs(np.cos(ang))
v2 = b*hand_normal
# Generate the pivoting center point by translating the surface center using the computed translation vectors
center = translate_point(surface.center_point,v1+v2)
return center
def place_finger(obj,d,z,normal,distal,hand_normal):
"""
Function to generate Geometry3D.ConvexPolygon definitions of fingers (including finger geometry and pose)
Parameters
----------
obj: dict
Dictionary including information about each surface on the object in the following form:
Geometry3D ConvexPolygon , Geometry3D Vector , Geometry3D ConvexPolygon
{surface_no:(surface_polygon_definition, surface_normal_vector, goal_region_polygon(if available))}
d: float
Parameter d for given finger
z: float
Elevation z for given finger
normal: Geometry3D.Vector
Normal vector pointing out of the finger (backhand)
distal: Geometry3D.Vector
Distal vector pointing out of the finger (from palm to fingertip)
hand_normal: Geometry3D.Vector
Vector normal to the manipulation plane of the hand
Returns
----------
finger: Geometry3D.ConvexPolygon
Finger geometry defined as a convex polygon - including pose information (position and orientation)
"""
# Loop through the object surfaces to find the one that has the same (similar) normal vector with the finger
for surf in obj:
if obj[surf][1].angle(normal)<1e-3:
# Contact surface found
surface = obj[surf][0]
break
# Find object length as the distance between two corners of the surface in the distal direction
for point in surface.points:
for other_point in surface.points:
if point==other_point:
continue
else:
edge = Vector(point,other_point)
angle = edge.angle(distal)
if angle <1e-3:
obj_length = edge.length()
# Find finger center by translating the surface center along z and distal direction using given finger parameters
finger_center = translate_point(surface.center_point,(hand_normal*z - distal*(d+obj_length/2-finger_w/2)))
# Find corner 1-4 of the finger by translating the finger center according to given finger parameters
finger_p1 = translate_point(finger_center,(distal*(finger_w/2)+distal.cross(normal)*(finger_h/2)))
finger_p2 = translate_point(finger_center,(-distal*(finger_w/2)+distal.cross(normal)*(finger_h/2)))
finger_p3 = translate_point(finger_center,(-distal*(finger_w/2)-distal.cross(normal)*(finger_h/2)))
finger_p4 = translate_point(finger_center,(distal*(finger_w/2)-distal.cross(normal)*(finger_h/2)))
# Define Geometry3D.ConvexPolygon for the finger using finger corners
finger = ConvexPolygon((finger_p1,finger_p2,finger_p3,finger_p4))
return finger
def get_finger_param(finger_poly,obj):
"""
Given the finger polygon and the object find the state parameters (finger parameters)
Parameters
----------
finger_poly: (surf, normal, distal): tuple
Finger polygon (Geometry3D.ConvexPolygon), finger normal(Geometry3D.Vector), distal vector (Geometry3D.Vector)
obj: dict
Dictionary including information about each surface on the object in the following form:
Geometry3D ConvexPolygon , Geometry3D Vector , Geometry3D ConvexPolygon
{surface_no:(surface_polygon_definition, surface_normal_vector, goal_region_polygon(if available))}
Returns
----------
d: float
Distance between finger joint and object start
z: float
Elevation of the finger on the object
"""
# Find finger center point
finger_center = finger_poly[0].center_point
# Find contact surface
for surf in obj:
if obj[surf][1].angle(finger_poly[1])<1e-3:
surface = obj[surf][0]
break
# Find object length alond finger distal vector
for point in surface.points:
for other_point in surface.points:
if point==other_point:
continue
else:
edge = Vector(point,other_point)
angle = edge.angle(finger_poly[2])
if angle <1e-3:
obj_length = edge.length()
# Find the center of the contact surface
obj_center = surface.center_point
# Center point on the proximal object edge
close_edge = translate_point(obj_center,(-finger_poly[2]*(obj_length/2)))
# Generate the vector connecting the proximal edge center and finger center
vec = Vector(finger_center,close_edge)
# If the vector has a length of 0, centers align, thus elevation is the same and d is half of the finger length
if vec.length()==0:
d = finger_w/2
z = 0
# Else compute the angle between distal vector and the generated vector and find the distance components to determine d and z
else:
ang = vec.angle(finger_poly[2])
if ang<np.pi/2:
b = vec.length()*abs(np.cos(ang))
else:
b = -vec.length()*abs(np.cos(ang))
d = np.round(b+finger_w/2,decimals=1)
q = obj_center.distance(finger_center)**2-(b+obj_length/2)**2
if q < 0 and abs(q) < 1e-3:
q = 0
if finger_poly[2].cross(finger_poly[1]).angle(vec)<np.pi/2:
z = -np.round(np.sqrt(q),decimals=1)
else:
z = np.round(np.sqrt(q),decimals=1)
# debugging material
if np.isnan(z):
print(obj_center.distance(finger_center))
print((b+obj_length/2))
print(obj_center)
print(finger_center)
print(b)
print(vec)
print(finger_poly[2])
return None
return d,z
