def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
aco = AttachedCollisionObject()
if pose!=None and not filename.empty():
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_mesh_custom(self, link, name, mesh, pose=None, touch_links=[]):
aco = AttachedCollisionObject()
if pose != None and mesh:
aco.object = self.__make_mesh_custom(name, pose, mesh, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
aco = AttachedCollisionObject()
if (pose is not None) and filename:
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self.__submit(aco, attach=True)
def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
aco = AttachedCollisionObject()
if pose!=None and not filename.empty():
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
aco = AttachedCollisionObject()
if (pose is not None) and filename:
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self.__submit(aco, attach=True)
def attach_box(self, link, name, pose = None, size = (1, 1, 1), touch_links = []):
aco = AttachedCollisionObject()
if pose!=None:
aco.object = self.__make_box(name, pose, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
self._pub_aco.publish(aco)
def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]):
aco = AttachedCollisionObject()
if pose is not None:
aco.object = self.__make_box(name, pose, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
self.__submit(aco, attach=True)
def publish_parsed_urdf(parsed_urdf, scene, ap, attach=False):
""" Publish link geometry for every joint's child. """
for name, joint in parsed_urdf["joints"].items():
# get the child link data
link = parsed_urdf["links"][joint["child"]]
# publish the child links collision geometry
if link["type"] == "box":
if attach:
scene.attach_box("table", joint["child"], joint["pose"],
link["size"], ["table"])
else:
scene.add_box(joint["child"], joint["pose"], link["size"])
elif link["type"] == "cylinder":
if attach:
aco = AttachedCollisionObject()
aco.touch_links = ["table"]
aco.object = make_cylinder(joint["child"], joint["pose"],
link["length"], link["radius"])
aco.link_name = "table"
print(aco.object)
ap.publish(aco)
else:
scene.add_cylinder(joint["child"], joint["pose"],
link["length"], link["radius"])
else:
scene.add_mesh(joint["child"], joint["pose"], link["filename"],
link["scale"])
def form_attach_collision_object_msg(self, link_name, object_name, size,
pose):
obj = AttachedCollisionObject()
# The CollisionObject will be attached with a fixed joint to this link
obj.link_name = link_name
# This contains the actual shapes and poses for the CollisionObject
# to be attached to the link
col_obj = CollisionObject()
col_obj.id = object_name
col_obj.header.frame_id = link_name
col_obj.header.stamp = rospy.Time.now()
sp = SolidPrimitive()
sp.type = sp.BOX
sp.dimensions = [0.0] * 3
sp.dimensions[0] = size.x
sp.dimensions[1] = size.y
sp.dimensions[2] = size.z
col_obj.primitives = [sp]
col_obj.primitive_poses = [pose]
# Adds the object to the planning scene. If the object previously existed, it is replaced.
col_obj.operation = col_obj.ADD
obj.object = col_obj
# The weight of the attached object, if known
obj.weight = 0.0
return obj
def publish_attached(self, attached_obj, side):
attached_obj.operation = CollisionObject.ADD
msg = AttachedCollisionObject()
msg.object = attached_obj
msg.link_name = side+"_gripper"
touch_links = [side+"_gripper", side+"_gripper_base", side+"_hand_camera", side+"_hand_range", "octomap"]
self.attached_pub.publish(msg)
def attach_box(self, link, name, pose, size=(1, 1, 1), touch_links=[]):
aco = AttachedCollisionObject()
aco.object = self.make_box(name, pose, size)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_mesh(self, link, name, pose, filename, touch_links=[]):
aco = AttachedCollisionObject()
aco.object = self.__make_mesh(name, pose, filename)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_mesh(self, link, name, pose, filename, touch_links = []):
aco = AttachedCollisionObject()
aco.object = self.__make_mesh(name, pose, filename)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def _add_payload_to_planning_scene_msg(self, payload):
msg=payload.payload_msg
co = CollisionObject()
co.id = payload.payload_msg.name
co.header.frame_id = msg.header.frame_id
payload.attached_link=msg.header.frame_id
urdf_root=self.urdf.get_root()
urdf_chain=self.urdf.get_chain(urdf_root, payload.attached_link, joints=True, links=False)
urdf_chain.reverse()
touch_links=[]
touch_root = None
for j_name in urdf_chain:
j=self.urdf.joint_map[j_name]
if j.type != "fixed":
break
touch_root=j.parent
def _touch_recurse(touch):
ret=[touch]
if touch in self.urdf.child_map:
for c_j,c in self.urdf.child_map[touch]:
if self.urdf.joint_map[c_j].type == "fixed":
ret.extend(_touch_recurse(c))
return ret
if touch_root is not None:
touch_links.extend(_touch_recurse(touch_root))
for i in xrange(len(msg.collision_geometry.mesh_resources)):
mesh_filename=urlparse.urlparse(resource_retriever.get_filename(msg.collision_geometry.mesh_resources[i])).path
mesh_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.mesh_poses[i]))
mesh_scale=msg.collision_geometry.mesh_scales[i]
mesh_scale = (mesh_scale.x, mesh_scale.y, mesh_scale.z)
mesh_msg = load_mesh_file_to_mesh_msg(mesh_filename, mesh_scale)
co.meshes.extend(mesh_msg)
co.mesh_poses.extend([mesh_pose]*len(mesh_msg))
for i in xrange(len(msg.collision_geometry.primitives)):
co.primitives.append(msg.collision_geometry.primitives[i])
primitive_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.primitive_poses[i]))
co.primitive_poses.append(primitive_pose)
aco = AttachedCollisionObject()
aco.link_name = payload.attached_link
aco.object = co
aco.touch_links = touch_links
aco.weight = msg.inertia.m
#self._pub_aco.publish(aco)
return aco
def publish_attached(self, attached_obj, side):
attached_obj.operation = CollisionObject.ADD
msg = AttachedCollisionObject()
msg.object = attached_obj
msg.link_name = side + "_gripper"
touch_links = [
side + "_gripper", side + "_gripper_base", side + "_hand_camera",
side + "_hand_range", "octomap"
]
self.attached_pub.publish(msg)
def makeAttached(self, link_name, obj, touch_links, detach_posture,
weight):
o = AttachedCollisionObject()
o.link_name = link_name
o.object = obj
if touch_links:
o.touch_links = touch_links
if detach_posture:
o.detach_posture = detach_posture
o.weight = weight
return o
def makeAttached(self, link_name, obj, touch_links, detach_posture,
weight):
o = AttachedCollisionObject()
o.link_name = link_name
o.object = obj
if touch_links:
o.touch_links = touch_links
if detach_posture:
o.detach_posture = detach_posture
o.weight = weight
return o
def attach(self, selected_id_index = 0): grasping_group = 'end_effector' touch_links = self.robot.get_link_names(group=grasping_group) aco = AttachedCollisionObject() aco.object = self.co_list[selected_id_index] aco.link_name = 'ee_link' if len(touch_links) > 0: aco.touch_links = touch_links else: aco.touch_links = ['ee_link'] self.__submit(aco, attach=True) return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=4)
def form_remove_all_attached_msg(self, link_name):
obj = AttachedCollisionObject()
# The CollisionObject will be attached with a fixed joint to this link
obj.link_name = link_name
col_obj = CollisionObject()
# If action is remove and no object.id is set, all objects
# attached to the link indicated by link_name will be removed
col_obj.operation = col_obj.REMOVE
obj.object = col_obj
return obj
def attach_object(self, group_name):
rospy.loginfo('attaching object to ' + group_name)
# select toolframe depending on group name
tool_frame = TOOL_FRAME_RIGHT if 'right' in group_name else TOOL_FRAME_LEFT
# pose for attached object in tool frame coordiantes
pose = Pose()
pose.position.z = 0.05
pose.orientation.w = 1.0
primitive = SolidPrimitive()
primitive.type = primitive.BOX
primitive.dimensions = [0.07, 0.07, 0.07]
o = CollisionObject()
o.header.frame_id = tool_frame
o.id = "handed_object"
o.primitives = [primitive]
o.primitive_poses = [pose]
o.operation = o.ADD
a = AttachedCollisionObject()
a.object = o
# allow collisions with hand links
a.touch_links = ALLOWED_TOUCH_LINKS
# attach object to tool frame
a.link_name = tool_frame
# don't delete old planning scene, if we didn't get one so far, create a new one
scene = self.current_planning_scene if self.current_planning_scene is not None else PlanningScene(
)
# add attached object to scene
scene.robot_state.attached_collision_objects.append(a)
# mark scene as changed
scene.is_diff = True
self.pub_ps.publish(scene)
def attach_object(self, group_name):
rospy.loginfo('attaching object to ' + group_name)
# select toolframe depending on group name
tool_frame = TOOL_FRAME_RIGHT if 'right' in group_name else TOOL_FRAME_LEFT
# pose for attached object in tool frame coordiantes
pose = Pose()
pose.position.z = 0.05
pose.orientation.w = 1.0
primitive = SolidPrimitive()
primitive.type = primitive.BOX
primitive.dimensions = [0.07, 0.07, 0.07]
o = CollisionObject()
o.header.frame_id = tool_frame
o.id = "handed_object"
o.primitives = [primitive]
o.primitive_poses = [pose]
o.operation = o.ADD
a = AttachedCollisionObject()
a.object = o
# allow collisions with hand links
a.touch_links = ALLOWED_TOUCH_LINKS
# attach object to tool frame
a.link_name = tool_frame
# don't delete old planning scene, if we didn't get one so far, create a new one
scene = self.current_planning_scene if self.current_planning_scene is not None else PlanningScene()
# add attached object to scene
scene.robot_state.attached_collision_objects.append(a)
# mark scene as changed
scene.is_diff = True
self.pub_ps.publish(scene)
def attach_sphere(link, name, pose, radius, touch_links=[]):
aco = AttachedCollisionObject()
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose.pose]
aco.object = co
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
scene._pub_aco.publish(aco)
def attach_cylinder(link, name, pose, height, radius, touch_links=[]):
aco = AttachedCollisionObject()
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
aco.object = co
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
scene._pub_aco.publish(aco)
def attachCylinder(self,link,name,pose,size,touch_links=[]):
"""
Attaches a cylinder to a gripper
:param name: Name of the new cylinder object
:type name: str
:param pose: Pose of the new object
:type pose: geometry_msgs.msg.PoseStamped
:param size: Dimensions of the new object (height[m],radius[m])
:type size: (float, float)
:param touch_links: links that are already in collision with the attached object (added links do not trigger a collision)
:type touch_links: list(str)
"""
#print "attach object with name", name
#print "touch links",touch_links
aco = AttachedCollisionObject()
aco.object = self.__make_cylinder(name, pose, size)
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
self.scene._pub_aco.publish(aco)
def init_cluster_grasper(self, cluster, probabilities=[], use_probability=True):
self.cluster_frame = cluster.header.frame_id
#use PCA to find the object frame and bounding box dims, and to get the cluster points in the object frame (z=0 at bottom of cluster)
if use_probability:
if len(probabilities) == 0:
(self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
self.object_to_base_frame, self.object_to_cluster_frame) = \
self.cbbf.find_object_frame_and_bounding_box(cluster, []) #empty probabilities
if self.draw_box:
self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_object_box', color=[0.2,0.2,1])
print 'probabilistic_object_box'
else:
(self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
self.object_to_base_frame, self.object_to_cluster_frame) = \
self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities)
#draw the bounding box
if self.draw_box:
self.draw_bounding_box(self.object_bounding_box_dims, name='probabilistic_weighted_object_box', color=[0,0.9,0.9])
print 'probabilistic_weighted_object_box'
else:
(self.object_points, self.object_bounding_box_dims, self.object_bounding_box, \
self.object_to_base_frame, self.object_to_cluster_frame) = \
self.cbbf.find_object_frame_and_bounding_box(cluster, probabilities)
if self.draw_box:
self.draw_bounding_box(self.object_bounding_box_dims, name='deterministic_object_box', color=[1,0,0])
print 'deterministic_object_box'
# MoveIt Stuff: CollisionObject
'''
print "self.object_bounding_box_dims=", self.object_bounding_box_dims
print "self.object_bounding_box=", self.object_bounding_box
print "self.object_to_base_frame.shape=", self.object_to_base_frame.shape
print "self.object_to_base_frame=", self.object_to_base_frame
print "self.object_to_cluster_frame=", self.object_to_cluster_frame
'''
# Add the bounding box as a CollisionObject
co = CollisionObject()
co.header.stamp = rospy.get_rostime()
#co.header.frame_id = "base_footprint"
co.header.frame_id = "base_link"
co.primitives.append(SolidPrimitive())
co.primitives[0].type = SolidPrimitive.BOX
# Clear the previous CollisionObject
co.id = "part"
co.operation = co.REMOVE
self.pub_co.publish(co)
# Clear the previously attached object
aco = AttachedCollisionObject()
aco.object = co
self.pub_aco.publish(aco)
# Add the new CollisionObject
box_height = self.object_bounding_box_dims[2]
co.operation = co.ADD
co.primitives[0].dimensions.append(self.object_bounding_box_dims[0])
co.primitives[0].dimensions.append(self.object_bounding_box_dims[1])
co.primitives[0].dimensions.append(self.object_bounding_box_dims[2])
co.primitive_poses.append(Pose())
co.primitive_poses[0].position.x = self.object_to_base_frame[0,3]
co.primitive_poses[0].position.y = self.object_to_base_frame[1,3]
co.primitive_poses[0].position.z = self.object_to_base_frame[2,3] + box_height/2
quat = tf.transformations.quaternion_about_axis(math.atan2(self.object_to_base_frame[1,0], self.object_to_base_frame[0,0]), (0,0,1))
#quat = tf.transformations.quaternion_from_matrix(self.object_to_base_frame)
co.primitive_poses[0].orientation.x = quat[0]
co.primitive_poses[0].orientation.y = quat[1]
co.primitive_poses[0].orientation.z = quat[2]
co.primitive_poses[0].orientation.w = quat[3]
self.pub_co.publish(co)
# END MoveIt! stuff
#for which directions does the bounding box fit within the hand?
gripper_space = [self.gripper_opening - self.object_bounding_box_dims[i] for i in range(3)]
self._box_fits_in_hand = [gripper_space[i] > 0 for i in range(3)]
#only half benefit for the longer dimension, if one is significantly longer than the other
if self._box_fits_in_hand[0] and self._box_fits_in_hand[1]:
if gripper_space[0] > gripper_space[1] and self.object_bounding_box_dims[0]/self.object_bounding_box_dims[1] < .8:
self._box_fits_in_hand[1] *= .5
elif gripper_space[1] > gripper_space[0] and self.object_bounding_box_dims[1]/self.object_bounding_box_dims[0] < .8:
self._box_fits_in_hand[0] *= .5
#rospy.loginfo("bounding box dims: "+pplist(self.object_bounding_box_dims))
#rospy.loginfo("self._box_fits_in_hand: "+str(self._box_fits_in_hand))
#compute the average number of points per sq cm of bounding box surface (half the surface only)
bounding_box_surf = (self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[1]*100) + \
(self.object_bounding_box_dims[0]*100 * self.object_bounding_box_dims[2]*100) + \
(self.object_bounding_box_dims[1]*100 * self.object_bounding_box_dims[2]*100)
self._points_per_sq_cm = self.object_points.shape[1] / bounding_box_surf
