def test_check_collision():
global is_exit
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('as_arm_check_collision', anonymous=True)
robot = moveit_commander.RobotCommander()
# arm_group = moveit_commander.MoveGroupCommander("arm")
scene = moveit_commander.PlanningSceneInterface()
print "==========links:", robot.get_link_names()
# scene.attach_box("camera_mast_link", "camera_mast_link")
check_state_validity = rospy.ServiceProxy('/check_state_validity',
GetStateValidity)
while not is_exit:
robot_state = robot.get_current_state()
col_obj = AttachedCollisionObject()
col_obj.link_name = "camera_mast_link"
robot_state.attached_collision_objects.append(col_obj)
robot_state.is_diff = True
print robot_state
group_name = ""
contraint = Constraints()
resp = check_state_validity(robot_state, group_name, contraint)
print "==================="
print resp
if not resp.valid:
print "not valid"
is_exit = True
rospy.sleep(0.5)
moveit_commander.roscpp_shutdown()
def detach_object(self, name):
detach_object = AttachedCollisionObject()
detach_object.object.id = name
detach_object.link_name = "l_wrist_roll_link"
detach_object.object.operation = detach_object.object.REMOVE
self.attach_pub.publish(detach_object)
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 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 virtualObjectLeftHand():
print "Creating a virtual object attached to gripper..."
# for more details refer to ROS docs for moveit_msgs/AttachedCollisionObject
object1 = AttachedCollisionObject()
object1.link_name = "left_HandGripLink"
object1.object.header.frame_id = "left_HandGripLink"
object1.object.id = "object1"
object1_prim = SolidPrimitive()
object1_prim.type = SolidPrimitive.CYLINDER
object1_prim.dimensions=[None, None] # set initial size of the list to 2
object1_prim.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = 0.23
object1_prim.dimensions[SolidPrimitive.CYLINDER_RADIUS] = 0.06
object1_pose = pm.toMsg(PyKDL.Frame(PyKDL.Rotation.RotY(math.pi/2)))
object1.object.primitives.append(object1_prim)
object1.object.primitive_poses.append(object1_pose)
object1.object.operation = CollisionObject.ADD
object1.touch_links = ['left_HandPalmLink',
'left_HandFingerOneKnuckleOneLink',
'left_HandFingerOneKnuckleTwoLink',
'left_HandFingerOneKnuckleThreeLink',
'left_HandFingerTwoKnuckleOneLink',
'left_HandFingerTwoKnuckleTwoLink',
'left_HandFingerTwoKnuckleThreeLink',
'left_HandFingerThreeKnuckleTwoLink',
'left_HandFingerThreeKnuckleThreeLink']
return object1
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 cucumber(cucumber, pose):
'''
Create a cylinder that represents the gripped cucumber.
@return An AttachedCollisionObject representing the roof
'''
aco = AttachedCollisionObject()
aco.link_name = "ee_link"
aco.object.header.frame_id = "world"
aco.object.id = "cucumber"
acoPose = pose
acoPose.position.y += cucumber.width / 2 + 0.15
acoPose.position.z -= cucumber.height / 2 + 0.071
acoCyl = SolidPrimitive()
acoCyl.type = acoCyl.CYLINDER
acoCyl.dimensions = [cucumber.height, cucumber.width / 2]
aco.object.primitives.append(acoCyl)
aco.object.primitive_poses.append(acoPose)
aco.object.operation = aco.object.ADD
return aco
def detach_object(self, name):
detach_object = AttachedCollisionObject()
detach_object.object.id = name
detach_object.link_name = "l_wrist_roll_link"
detach_object.object.operation = detach_object.object.REMOVE
self.attach_pub.publish(detach_object)
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 remove_attached_object(self, link, name = ''):
"""
Remove object from planning scene
"""
aco = AttachedCollisionObject()
aco.object.operation = CollisionObject.REMOVE
aco.link_name = link
aco.object.id = name
self._pub_aco.publish(aco)
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
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
self._pub_aco.publish(aco)
def remove_attached_object(self, link, name=''):
"""
Remove object from planning scene
"""
aco = AttachedCollisionObject()
aco.object.operation = CollisionObject.REMOVE
aco.link_name = link
aco.object.id = name
self._pub_aco.publish(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 remove_attached_object(self, link, name = None):
"""
Remove an attached object from planning scene, or all objects attached to this link if no name is provided
"""
aco = AttachedCollisionObject()
aco.object.operation = CollisionObject.REMOVE
aco.link_name = link
if name != None:
aco.object.id = name
self._pub_aco.publish(aco)
def remove_attached_object(self, link, name=None):
"""
Remove an attached object from planning scene, or all objects attached to this link if no name is provided
"""
aco = AttachedCollisionObject()
aco.object.operation = CollisionObject.REMOVE
aco.link_name = link
if name != None:
aco.object.id = name
self._pub_aco.publish(aco)
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 detach_object(self, name='', link=''):
"""Given the name of a link, detach the object(s) from that link.
Args:
@name (str): object name (detach all attached objects if avoided)
@link (str): link to detach from
"""
aoc = AttachedCollisionObject()
aoc.object.operation = CollisionObject.REMOVE
if name:
aoc.object.id = name
if link:
aoc.link_name = link
self._apply_scene_diff([aoc])
def _remove_payload_from_planning_scene_msg(self, payload):
msg=payload.payload_msg
aco = AttachedCollisionObject()
aco.object.operation = CollisionObject.REMOVE
if payload.attached_link is not None:
aco.link_name = payload.attached_link
else:
aco.link_name = ""
aco.object.id = msg.name
#self._pub_aco.publish(aco)
payload.attached_link = None
return aco
def attach_object(self, name, link='', touch_links=[]):
"""Given the name of an object existing in the planning scene,
attach it to a robot link.
Args:
@name (str): collision object name
@link (str): link to attach to
@touch_links (list): links that can touch an object
"""
aoc = AttachedCollisionObject()
aoc.object.operation = CollisionObject.ADD
aoc.object.id = name
aoc.touch_links = touch_links
aoc.link_name = link
self._apply_scene_diff([aoc])
def remove_attached_object(self, link=None, name=None):
"""
Remove an attached object from the robot, or all objects attached to the link if no name is provided,
or all attached objects in the scene if neither link nor name are provided.
Removed attached objects remain in the scene as world objects.
Call remove_world_object afterwards to remove them from the scene.
"""
aco = AttachedCollisionObject()
aco.object.operation = CollisionObject.REMOVE
if link is not None:
aco.link_name = link
if name is not None:
aco.object.id = name
self.__submit(aco, attach=True)
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 get_attached_objects(self, object_ids=[]):
"""
Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects.
"""
ser_aobjs = self._psi.get_attached_objects(object_ids)
aobjs = dict()
for key in ser_aobjs:
msg = AttachedCollisionObject()
conversions.msg_from_string(msg, ser_aobjs[key])
aobjs[key] = msg
return aobjs
def remCucumber():
'''
Remove a cylinder that represents the gripped cucumber.
@return An AttachedCollisionObject representing the roof
'''
aco = AttachedCollisionObject()
aco.object.header.frame_id = "world"
aco.object.id = "cucumber"
aco.object.operation = aco.object.REMOVE
return aco
def table():
'''
Create the table and back plate that is connected to the robot base.
@return An AttachedCollisionObject representing the table and back plate
'''
HEIGHT_OFFSET = 0.004
table = AttachedCollisionObject()
table.link_name = "base_link"
table.object.header.frame_id = "/world"
table.object.id = "table"
tablePose = Pose()
tablePose.position.x -= 0.5
tablePose.position.z = -0.1 / 2 - HEIGHT_OFFSET #- 0.26
tablePose.orientation.w = 1.0
tableBox = SolidPrimitive()
tableBox.type = tableBox.BOX
tableBox.dimensions = [1.5, 1, 0.1]
table.object.primitives.append(tableBox)
table.object.primitive_poses.append(tablePose)
backpanelPose = Pose()
backpanelPose.position.y = -0.5
backpanelPose.position.z = 0.5 - HEIGHT_OFFSET - 0.26
backpanelPose.orientation.w = 1.0
backpanelBox = SolidPrimitive()
backpanelBox.type = backpanelBox.BOX
backpanelBox.dimensions = [1.5, 0.1, 1]
table.object.primitives.append(backpanelBox)
table.object.primitive_poses.append(backpanelPose)
table.object.operation = table.object.ADD
return table
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 removeAttachedObject(self, name, use_service=True):
""" Remove an attached object. """
o = AttachedCollisionObject()
o.object.operation = CollisionObject.REMOVE
o.object.id = name
try:
del self._attached_objects[name]
self._attached_removed[name] = o
except KeyError:
pass
self.sendUpdate(None, o, use_service)
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_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, 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_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 removeAttachedObject(self, name, wait=True):
""" Remove an attached object. """
o = AttachedCollisionObject()
o.object.operation = CollisionObject.REMOVE
#o.link_name = ??
o.object.id = name
try:
del self._attached_objects[name]
self._attached_removed[name] = o
except KeyError:
pass
self.sentUpdate(None, o, wait)
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 _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 roof():
'''
Create a roof that constrains the random movements of the robot.
@return An AttachedCollisionObject representing the roof
'''
roof = AttachedCollisionObject()
roof.link_name = "base_link"
roof.object.header.frame_id = "world"
roof.object.id = "roof"
roofPose = Pose()
roofPose.position.z = .6
roofPose.orientation.w = 1.0
roofBox = SolidPrimitive()
roofBox.type = roofBox.BOX
roofBox.dimensions = [1.5, 1, 0.025]
roof.object.primitives.append(roofBox)
roof.object.primitive_poses.append(roofPose)
return roof
def removeAttachedObject(self, name, wait=True):
""" Remove an attached object. """
o = AttachedCollisionObject()
o.object.operation = CollisionObject.REMOVE
#o.link_name = ??
o.object.id = name
try:
del self._attached_objects[name]
self._attached_removed[name] = o
except KeyError:
pass
self._attached_pub.publish(o)
if wait:
self.waitForSync()
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 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_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 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