def grasp_trans(self, oracle):
aabb = oracle.get_aabb(oracle.get_body_name(self.geom_hash))
manip_point = aabb.pos() + np.array([0, 0, aabb.extents()[2] - self.z_distance])
tool_quat = quat_from_trans(get_tool_trans(oracle))
for rotation in np.linspace(0, 2*PI, 4, endpoint=False):
manip_quat = quat_dot(quat_look_at(-unit_z()), quat_from_angle_vector(rotation, unit_z()), tool_quat) # Grip * Tool = Manip
yield compute_grasp(trans_from_quat_point(manip_quat, manip_point), unit_trans()), self.approach_vector
def grasp_trans(self, oracle):
geometry = geometries(oracle.get_body(oracle.get_body_name(self.geom_hash)))[0]
manip_point = point_from_trans(geometry.GetTransform()) + \
np.array([0, 0, .5*geometry.GetCylinderHeight()[2] + self.z_distance])
tool_quat = quat_from_trans(get_tool_trans(oracle))
for rotation in np.linspace(0, 2*PI, self.number, endpoint=False):
manip_quat = quat_dot(quat_look_at(-unit_z()), quat_from_angle_vector(rotation, unit_z()), tool_quat) # Grip * Tool = Manip
yield compute_grasp(trans_from_quat_point(manip_quat, manip_point), unit_trans()), self.approach_vector
def grasp_trans(self, oracle):
geometry = geometries(
oracle.get_body(oracle.get_body_name(self.geom_hash)))[0]
manip_point = point_from_trans(geometry.GetTransform()) + \
np.array([0, 0, .5*geometry.GetCylinderHeight()[2] + self.z_distance])
tool_quat = quat_from_trans(get_tool_trans(oracle))
for rotation in np.linspace(0, 2 * PI, self.number, endpoint=False):
manip_quat = quat_dot(quat_look_at(-unit_z()),
quat_from_angle_vector(rotation, unit_z()),
tool_quat) # Grip * Tool = Manip
yield compute_grasp(trans_from_quat_point(manip_quat, manip_point),
unit_trans()), self.approach_vector
def grasp_trans(self, oracle): # TODO - top / bottom, sample different heights, and infinite generator
# NOTE - grasp max radius is about .035
geometry = geometries(oracle.get_body(oracle.get_body_name(self.geom_hash)))[0]
pos = point_from_trans(geometry.GetTransform())
tool_quat = quat_from_trans(get_tool_trans(oracle))
for theta in np.linspace(0, 2*PI, self.number, endpoint=False):
direction = np.array([cos(theta), sin(theta), 0])
manip_point = self.r_distance*direction + pos
for rotation in [0, PI]:
manip_quat = quat_dot(quat_look_at(-direction), quat_from_angle_vector(rotation, unit_x()), tool_quat) # Grip * Tool = Manip
approach_vector = quat_transform_point((quat_from_z_rot(theta)), self.approach_vector)
yield compute_grasp(trans_from_quat_point(manip_quat, manip_point), unit_trans()), approach_vector
def grasp_trans(self, oracle):
aabb = oracle.get_aabb(oracle.get_body_name(self.geom_hash))
manip_point = aabb.pos() + np.array(
[0, 0, aabb.extents()[2] - self.z_distance])
tool_quat = quat_from_trans(get_tool_trans(oracle))
#print np.linspace(0, 2*PI, 4, endpoint=False)
for rotation in np.linspace(
0, 2 * PI, 4, endpoint=False
): # I think the rotation is the rotation of the manipulator
#manip_quat = quat_dot(quat_look_at(-unit_z()), quat_from_angle_vector(rotation, unit_z()), tool_quat) # NOTE - previously was a bug
manip_quat = quat_dot(quat_look_at(-unit_z()),
quat_from_angle_vector(rotation, unit_x()),
tool_quat) # Grip * Tool = Manip
yield compute_grasp(trans_from_quat_point(manip_quat, manip_point),
unit_trans()), self.approach_vector
def grasp_trans(self, oracle): # TODO - top / bottom and sample different heights
# NOTE - in the grasp transform (!= gripper transform)
# +z is the outwards pointing direction of the gripper
# +x is perpendicular to the gripper's plane
# Max width to grasp is about 0.07
aabb = oracle.get_aabb(oracle.get_body_name(self.geom_hash)) # body.GetLinks()[0].GetGeometries()[0].GetBoxExtents()
radii = np.tile(np.array([aabb.extents()[i]-self.xy_distance for i in range(2)]), 2)
z = aabb.extents()[2]-self.z_distance
thetas = np.linspace(0, 2*PI, 4, endpoint=False)
tool_quat = quat_from_trans(get_tool_trans(oracle))
for radius, theta in zip(radii, thetas):
direction = np.array([cos(theta), sin(theta), 0])
manip_point = radius*direction + np.array([0, 0, z]) + aabb.pos()
for rotation in [0, PI]:
manip_quat = quat_dot(quat_look_at(-direction), quat_from_angle_vector(rotation, unit_x()), tool_quat) # Grip * Tool = Manip
approach_vector = quat_transform_point((quat_from_z_rot(theta)), self.approach_vector)
yield compute_grasp(trans_from_quat_point(manip_quat, manip_point), unit_trans()), approach_vector
def get_contacts(oracle, body_name, direction): # NOTE - Other objects will only have a fixed set of contacts and directions
#print 'direction', direction[2]
assert direction[2] == 0
contacts = []
direction = normalize(direction)
aabb = oracle.get_aabb(body_name)
radius = sqrt(oracle.get_radius2D2(body_name))
#radius = body.GetLinks()[0].GetGeometries()[0].GetCylinderRadius()
height = 2*aabb.extents()[2]
#height = body.GetLinks()[0].GetGeometries()[0].GetCylinderHeight()
distance = radius + PUSH_SEPERATION
z = -height/2 + PUSH_HEIGHT
tool_quat = quat_from_trans(get_tool_trans(oracle))
manip_point = -distance*direction + np.array([0, 0, z]) + aabb.pos()
for rotation in [0, PI]: # NOTE - 2 hand trans can push in a given direction
manip_quat = quat_dot(quat_look_at(-direction), quat_look_at(-unit_z()), quat_from_angle_vector(rotation, unit_x()), tool_quat) # Grip * Tool = Manip
contacts.append(Contact(compute_grasp(trans_from_quat_point(manip_quat, manip_point), unit_trans()), direction))
return contacts
def grasp_trans(
self, oracle): # TODO - top / bottom and sample different heights
# NOTE - in the grasp transform (!= gripper transform)
# +z is the outwards pointing direction of the gripper
# +x is perpendicular to the gripper's plane
# Max width to grasp is about 0.07
#NOTE: this assumes that object is at the origin
aabb = oracle.get_aabb(
oracle.get_body_name(self.geom_hash)
) # body.GetLinks()[0].GetGeometries()[0].GetBoxExtents()
radii = np.tile(
np.array([aabb.extents()[i] - self.xy_distance for i in range(2)]),
2)
z = aabb.extents()[2] - self.z_distance
thetas = np.linspace(0, 2 * PI, 4, endpoint=False)
print 'Side grasp generation'
manip_point = aabb.pos() + np.array(
[0, 0, aabb.extents()[2] - self.z_distance])
tool_quat = quat_from_trans(get_tool_trans(oracle))
for rotation in np.linspace(
0, 2 * PI, 4, endpoint=False
): # I think the rotation is the rotation of the manipulator
#approach_vector = quat_transform_point((quat_from_z_rot(theta)), self.approach_vector)
manip_quat = quat_dot(quat_look_at(-unit_z()),
quat_from_angle_vector(rotation, unit_x()),
tool_quat) # Grip * Tool = Manip
"""
print '==============='
print oracle.robot.GetActiveManipulator().GetLocalToolTransform()
print tool_quat
print rotation
print aabb
print self.approach_vector
print manip_quat
print manip_point
print '==============='
"""
yield compute_grasp(trans_from_quat_point(manip_quat, manip_point),
unit_trans()), self.approach_vector
"""
def get_contacts(oracle, body_name, direction): # NOTE - Other objects will only have a fixed set of contacts and directions
#print 'direction', direction[2]
assert direction[2] == 0
contacts = []
direction = normalize(direction)
aabb = oracle.get_aabb(body_name)
radius = sqrt(oracle.get_radius2D2(body_name))
#radius = body.GetLinks()[0].GetGeometries()[0].GetCylinderRadius()
height = 2*aabb.extents()[2]
#height = body.GetLinks()[0].GetGeometries()[0].GetCylinderHeight()
distance = radius + PUSH_SEPERATION
z = -height/2 + PUSH_HEIGHT
tool_quat = quat_from_trans(get_tool_trans(oracle))
manip_point = -distance*direction + np.array([0, 0, z]) + aabb.pos()
for rotation in [0, PI]: # NOTE - 2 hand trans can push in a given direction
manip_quat = quat_dot(quat_look_at(-direction), quat_look_at(-unit_z()), quat_from_angle_vector(rotation, unit_x()), tool_quat) # Grip * Tool = Manip
contacts.append(Contact(compute_grasp(trans_from_quat_point(manip_quat, manip_point), unit_trans()), direction))
return contacts
# def box_contacts(oracle, body_name): # TODO - push boxes along their faces
def grasp_trans(
self, oracle
): # TODO - top / bottom, sample different heights, and infinite generator
# NOTE - grasp max radius is about .035
geometry = geometries(
oracle.get_body(oracle.get_body_name(self.geom_hash)))[0]
pos = point_from_trans(geometry.GetTransform())
tool_quat = quat_from_trans(get_tool_trans(oracle))
for theta in np.linspace(0, 2 * PI, self.number, endpoint=False):
direction = np.array([cos(theta), sin(theta), 0])
manip_point = self.r_distance * direction + pos
for rotation in [0, PI]:
manip_quat = quat_dot(quat_look_at(-direction),
quat_from_angle_vector(
rotation, unit_x()),
tool_quat) # Grip * Tool = Manip
approach_vector = quat_transform_point(
(quat_from_z_rot(theta)), self.approach_vector)
yield compute_grasp(
trans_from_quat_point(manip_quat, manip_point),
unit_trans()), approach_vector
