def update_scene(self, points):
""" for debugging purposes, sends object, table and octomap clearing in same message"""
ps = PlanningSceneWorld()
# clear the octomap
ps.octomap.header.frame_id = 'odom_combined'
ps.octomap.octomap.id = 'OcTree'
# add object bounding box
(box_pose, box_dims) = self.get_bounding_box(points)
bbox_primitive = SolidPrimitive()
bbox_primitive.type = bbox_primitive.BOX
bbox_primitive.dimensions = [box_dims.x, box_dims.y, box_dims.z]
bbox = CollisionObject()
bbox.id = 'obj1'
bbox.header.frame_id = box_pose.header.frame_id
bbox.operation = bbox.ADD
bbox.primitives.append(bbox_primitive)
bbox.primitive_poses.append(box_pose.pose)
ps.collision_objects.append(bbox)
# and, table
table_pose = PoseStamped()
table_pose.header.frame_id = "odom_combined"
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = 0.75
table_pose.pose.orientation.w = 1.0
table_dims = Point()
table_dims.x = 0.7
table_dims.y = 1.0
table_dims.z = 0.05
table_primitive = SolidPrimitive()
table_primitive.type = table_primitive.BOX
# TODO: this is a hack to make the buffer bigger.
# By default, it appears to be 10cm x 10cm x 0cm, but I'm not sure which dimension is which
table_primitive.dimensions = [table_dims.x, table_dims.y, table_dims.z]
table = CollisionObject()
table.id = 'table'
table.header.frame_id = table_pose.header.frame_id
table.operation = table.ADD
table.primitives.append(table_primitive)
table.primitive_poses.append(table_pose.pose)
ps.collision_objects.append(table)
self.scene_diff_pub.publish(ps)
def add_invisible_collision_object(self):
co_box = CollisionObject()
co_box.header.frame_id = 'base_footprint'
co_box.id = 'invisible_box'
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box_height = 0.76
box.dimensions.append(0.80)
box.dimensions.append(1.60)
box.dimensions.append(box_height)
co_box.primitives.append(box)
box_pose = Pose()
box_pose.position.x = 0.80
box_pose.position.y = 0.0
box_pose.position.z = box_height / 2.0
box_pose.orientation.w = 1.0
co_box.primitive_poses.append(box_pose)
co_box.operation = CollisionObject.ADD
color = ObjectColor()
color.id = 'invisible_box'
color.color.g = 1.0
color.color.a = 0.15
ps = PlanningScene()
ps.world.collision_objects.append(co_box)
ps.object_colors.append(color)
ps.is_diff = True
try:
self.planning_scene_service(ps)
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
def add_padded_lab():
rospy.loginfo("Adding padded order bin")
poses = []
objects = []
o_b_pose = Pose()
o_b_pose.orientation = Quaternion(x=0, y=0, z=0, w=1)
o_b_pose.position = Point(x=0.5, y=0.2, z=0.22)
objects.append(
SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.43, 0.68, 0.46]))
poses.append(o_b_pose)
wiring_pose = Pose()
wiring_pose.orientation = Quaternion(x=0, y=0, z=0, w=1)
wiring_pose.position = Point(x=-.55, y=0, z=0.15)
objects.append(
SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.6, 0.7, 0.4]))
poses.append(wiring_pose)
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "padded_lab"
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
co.primitives = objects
co.primitive_poses = poses
scene._pub_co.publish(co)
def remove_object(self, name):
remove_object = CollisionObject()
remove_object.header.frame_id = "odom_combined"
remove_object.id = name
remove_object.operation = remove_object.REMOVE
self.collision_pub.publish(remove_object)
def remove_object(self, name):
remove_object = CollisionObject()
remove_object.header.frame_id = "odom_combined"
remove_object.id = name
remove_object.operation = remove_object.REMOVE
self.collision_pub.publish(remove_object)
def make_mesh(name, pose, filename, scale=(1, 1, 1)):
'''
returns a CollisionObject with the mesh defined in the path.
mostly copied from https://github.com/ros-planning/moveit_commander/blob/kinetic-devel/src/moveit_commander/planning_scene_interface.py
'''
print("Creating mesh with file from", filename)
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face) == 3:
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def _remove_command(self, name):
co = CollisionObject()
co.header.stamp = rospy.Time.now()
co.header.frame_id = self._planning_frame
co.id = name
co.operation = CollisionObject.REMOVE
return co
def create_collision_object(shape_type,pos,size,frame_id,op,object_id): col = CollisionObject() col.id = object_id col.operation = op col.header.frame_id=frame_id # create primitive primitive = SolidPrimitive() primitive.type = shape_type primitive.dimensions = size # create pose pose = Pose() pose.position.x = pos[0] pose.position.y = pos[1] pose.position.z = pos[2] pose.orientation.x = pose.orientation.y = pose.orientation.z = 0 pose.orientation.w = 1 col.primitives = [primitive] col.primitive_poses = [pose] return col
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 remove_object(name="Object"):
co = CollisionObject()
co.operation = CollisionObject.REMOVE
co.id = name
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
while scene._pub_co.get_num_connections() == 0:
rospy.sleep(0.01)
scene._pub_co.publish(co)
while True:
rospy.sleep(0.1)
result, success = get_planning_scene(
PlanningSceneComponents(
PlanningSceneComponents.WORLD_OBJECT_NAMES
+ PlanningSceneComponents.WORLD_OBJECT_GEOMETRY
)
)
if not success:
continue
found = False
for object in result.scene.world.collision_objects:
if object.id == name:
found = True
if not found:
return
def add_box_obstacle(self, size, name, pose):
"""
Adds a rectangular prism obstacle to the planning scene
Inputs:
size: 3x' ndarray; (x, y, z) size of the box (in the box's body frame)
name: unique name of the obstacle (used for adding and removing)
pose: geometry_msgs/PoseStamped object for the CoM of the box in relation to some frame
"""
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = size
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [pose.pose]
# Publish the object
self._planning_scene_publisher.publish(co)
def remove_object(self, name, type, costmap='both'):
"""
Remove an object from one or both costmaps.
:param name: object name
:param type: object type
:param costmap: 'local', 'global' or 'both'
"""
co = CollisionObject()
co.operation = CollisionObject.REMOVE
co.id = name
co.type.db = json.dumps({'table': 'NONE', 'type': type, 'name': name})
try:
if costmap in ['local', 'both']:
resp = self._lcm_sl_srv([co])
if resp.error.code != ThorpError.SUCCESS:
rospy.logerr(
"Unable to remove object %s from %s costmap: %d", name,
costmap, resp.error.code)
return False
if costmap in ['global', 'both']:
resp = self._gcm_sl_srv([co])
if resp.error.code != ThorpError.SUCCESS:
rospy.logerr(
"Unable to remove object %s from %s costmap: %d", name,
costmap, resp.error.code)
return False
rospy.loginfo("Removed object %s of type %s from %s costmap", name,
type, costmap)
return True
except rospy.ServiceException as err:
rospy.logerr("Unable to remove object %s from %s costmap: %s",
name, costmap, str(err))
return False
def __add_free_space(name, x, y, operation=CollisionObject.ADD):
sp = SolidPrimitive()
sp.type = SolidPrimitive.BOX
sp.dimensions = [0.5, 0.5, 0.1]
spp = PoseStamped()
spp.header.frame_id = 'map'
spp.pose.position.x = x
spp.pose.position.y = y
spp.pose.position.z = 0.05
spp.pose.orientation.w = 1
co = CollisionObject()
co.operation = operation
co.id = name
co.type.db = json.dumps({
'table': 'NONE',
'type': 'free_space',
'name': co.id
})
co.header.frame_id = 'map'
co.header.stamp = rospy.get_rostime()
co.primitives.append(sp)
co.primitive_poses.append(TF2().transform_pose(spp, spp.header.frame_id,
co.header.frame_id).pose)
psw = PlanningSceneWorld()
psw.collision_objects.append(co)
obstacle_pub.publish(psw)
rospy.loginfo("Added a fake obstacle named '%s'", name)
def makeMesh(self, name, pose, filename):
if not use_pyassimp:
rospy.logerr(
'pyassimp is broken on your platform, cannot load meshes')
return
scene = pyassimp.load(filename)
if not scene.meshes:
rospy.logerr('Unable to load mesh')
return
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
pyassimp.release(scene)
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = self._fixed_frame
o.id = name
o.meshes.append(mesh)
o.mesh_poses.append(pose)
o.operation = o.ADD
return o
def publishCO(self, points, id, is_table=False):
pts = np.asarray(points)
dx, dy, dz = np.max(pts, axis=0) - np.min(pts, axis=0)
max_x, max_y, max_z = np.max(pts, axis=0)
min_x, min_y, min_z = np.min(pts, axis=0)
collision_object = CollisionObject()
collision_object.id = id
collision_object.header = self.lastHeader
object_shape = SolidPrimitive()
object_shape.type = SolidPrimitive.BOX
object_shape.dimensions.append(dx)
object_shape.dimensions.append(dy)
shape_pose = Pose()
shape_pose.position.x = min_x + dx / 2
shape_pose.position.y = min_y + dy / 2
if is_table:
object_shape.dimensions.append(max_z)
shape_pose.position.z = max_z / 2
else:
object_shape.dimensions.append(dz)
shape_pose.position.z = min_z + dz / 2
shape_pose.orientation.w = 1.0
collision_object.primitives.append(object_shape)
collision_object.primitive_poses.append(shape_pose)
collision_object.operation = CollisionObject.ADD
self.pub_collision.publish(collision_object)
def makeMesh(self, name, pose, filename):
scene = pyassimp.load(filename)
if not scene.meshes:
rospy.logerr('Unable to load mesh')
return
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0],
face.indices[1],
face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
pyassimp.release(scene)
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = self._fixed_frame
o.id = name
o.meshes.append(mesh)
o.mesh_poses.append(pose)
o.operation = o.ADD
return o
def _add_box(self, name, px, py, pz, sl, sw, sh):
"""
Performs the simple adding of a single box with given dimensions.
"""
obj = CollisionObject()
obj.id = name
obj.operation = obj.ADD
obj.header.frame_id = "odom_combined"
obstacle = SolidPrimitive()
obstacle.type = obstacle.BOX
obstacle.dimensions = (sl, sw, sh)
pos = Pose()
pos.position.x = px
pos.position.y = py
pos.position.z = pz
pos.orientation.x = 0
pos.orientation.y = 0
pos.orientation.z = 0
pos.orientation.w = 1
obj.primitives.append(obstacle)
obj.primitive_poses.append(pos)
self.diff.world.collision_objects.append(obj)
def __make_mesh(self, name, pose, filename):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]
point.y = vertex[1]
point.z = vertex[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def __make_mesh(self, name, pose, filename, scale = (1, 1, 1)):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]*scale[0]
point.y = vertex[1]*scale[1]
point.z = vertex[2]*scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def _add_box(self, name, px, py, pz, sl, sw, sh):
"""
Performs the simple adding of a single box with given dimensions.
"""
obj = CollisionObject()
obj.id = name
obj.operation = obj.ADD
obj.header.frame_id = "odom_combined"
obstacle = SolidPrimitive()
obstacle.type = obstacle.BOX
obstacle.dimensions = (sl, sw, sh)
pos = Pose()
pos.position.x = px
pos.position.y = py
pos.position.z = pz
pos.orientation.x = 0
pos.orientation.y = 0
pos.orientation.z = 0
pos.orientation.w = 1
obj.primitives.append(obstacle)
obj.primitive_poses.append(pos)
self.diff.world.collision_objects.append(obj)
def remove_world_object(self, name):
"""
Remove object from planning scene
"""
co = CollisionObject()
co.operation = CollisionObject.REMOVE
co.id = name
self._pub_co.publish(co)
def _moveObject(self, objectName, objectPose):
co = CollisionObject()
co.operation = co.MOVE
co.id = objectName
co.mesh_poses = [objectPose.pose]
co.header.stamp = objectPose.header.stamp
co.header.frame_id = objectPose.header.frame_id
self._co_publisher.publish(co)
def _moveObject(self, objectName, objectPose):
co = CollisionObject()
co.operation = co.MOVE
co.id = objectName
co.mesh_poses = [objectPose.pose]
co.header.stamp = objectPose.header.stamp
co.header.frame_id = objectPose.header.frame_id
self._co_publisher.publish(co)
def move_object(self, name, pose):
co = CollisionObject()
co.operation = CollisionObject.MOVE
co.id = name
co.header = pose.header
co.primitive_poses = [pose.pose]
#sphere.dimensions = [radius] # future
self.__submit(co, attach=False)
def remove_world_object(self, name):
"""
Remove object from planning scene
"""
co = CollisionObject()
co.operation = CollisionObject.REMOVE
co.id = name
self._pub_co.publish(co)
def remove_world_object(self, name = None):
"""
Remove an object from planning scene, or all if no name is provided
"""
co = CollisionObject()
co.operation = CollisionObject.REMOVE
if name != None:
co.id = name
self._pub_co.publish(co)
def remove_world_object(self, name=None):
"""
Remove an object from planning scene, or all if no name is provided
"""
co = CollisionObject()
co.operation = CollisionObject.REMOVE
if name is not None:
co.id = name
self.__submit(co, attach=False)
def remove_world_object(self, name=None):
"""
Remove an object from planning scene, or all if no name is provided
"""
co = CollisionObject()
co.operation = CollisionObject.REMOVE
if name is not None:
co.id = name
self.__submit(co, attach=False)
def makeObject(self, shape, pose, solid): obj = CollisionObject() obj.header.stamp = rospy.Time.now() obj.id = shape obj.primitives.append(solid) obj.primitive_poses.append(pose) obj.operation = obj.ADD return obj
def remove_world_object(self, name=None):
"""
Remove an object from planning scene, or all if no name is provided
"""
co = CollisionObject()
co.operation = CollisionObject.REMOVE
if name != None:
co.id = name
self._pub_co.publish(co)
def __remove_virtual_obstacle(name):
co = CollisionObject()
co.operation = CollisionObject.REMOVE
co.id = name
co.type.db = json.dumps({'table': 'NONE', 'type': 'obstacle', 'name': co.id})
psw = PlanningSceneWorld()
psw.collision_objects.append(co)
obstacle_pub.publish(psw)
rospy.loginfo("Removed a fake obstacle named '%s'", name)
def makeSolidPrimitive(self, name, solid, pose):
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = self._fixed_frame
o.id = name
o.primitives.append(solid)
o.primitive_poses.append(pose)
o.operation = o.ADD
return o
def makeSolidPrimitive(self, name, solid, pose):
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = self._fixed_frame
o.id = name
o.primitives.append(solid)
o.primitive_poses.append(pose)
o.operation = o.ADD
return o
def _getCollisionObject(name, urdf, pose, operation):
'''
This function takes an urdf and a TF pose and updates the collision
geometry associated with the current planning scene.
'''
co = CollisionObject()
co.id = name
co.operation = operation
if len(urdf.links) > 1:
rospy.logwarn(
'collison object parser does not currently support kinematic chains'
)
for l in urdf.links:
# Compute the link pose based on the origin
if l.origin is None:
link_pose = pose
else:
link_pose = pose * kdl.Frame(kdl.Rotation.RPY(*l.origin.rpy),
kdl.Vector(*l.origin.xyz))
for c in l.collisions:
# Only update the geometry if we actually need to add the
# object to the collision scene.
# check type of each collision tag.
if isinstance(c.geometry, urdf_parser_py.urdf.Box):
primitive = True
if co.operation == CollisionObject.ADD:
size = c.geometry.size
element = SolidPrimitive()
element.type = SolidPrimitive.BOX
element.dimensions = list(c.geometry.size)
co.primitives.append(element)
elif isinstance(c.geometry, urdf_parser_py.urdf.Mesh):
primitive = False
if co.operation == CollisionObject.ADD:
scale = (1, 1, 1)
if c.geometry.scale is not None:
scale = c.scale
element = _loadMesh(c.geometry, scale)
co.meshes.append(element)
else:
raise NotImplementedError(
"we do not currently support geometry of type %s" %
(str(type(c.geometry))))
pose = kdl.Frame(kdl.Rotation.RPY(*c.origin.rpy),
kdl.Vector(*c.origin.xyz))
pose = link_pose * pose
if primitive:
co.primitive_poses.append(pm.toMsg(pose))
else:
# was a mesh
co.mesh_poses.append(pm.toMsg(pose))
return co
def __make_mesh_custom(self, name, pose, mesh):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
return co
def __init__(self, speed):
self.speed = speed
# Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the robot
self.robot = moveit_commander.RobotCommander()
# Initialize the planning scene
self.scene = moveit_commander.PlanningSceneInterface()
self.scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
# Instantiate a move group
self.group = moveit_commander.MoveGroupCommander(GROUP_NAME)
# Set the maximum time MoveIt will try to plan before giving up
self.group.set_planning_time(.25)
# Set maximum velocity scaling
self.group.set_max_velocity_scaling_factor(1.0)
self.group.set_max_acceleration_scaling_factor(1.0)
# For displaying plans to RVIZ
self.display_pub = rospy.Publisher('/move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
print(self.group.get_current_pose())
print(self.group.get_end_effector_link())
print(self.group.get_pose_reference_frame())
print(self.group.get_goal_tolerance())
# Create a CollisionObject, which will be added to the planning scene
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = 'table'
co.header = TABLE_CENTER.header
# Create a box primitive, which will be inside the CollisionObject
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = TABLE_SIZE
# Fill the collision object with primitive(s)
co.primitives = [box]
co.primitive_poses = [TABLE_CENTER.pose]
# Publish the object (don't know why but need to publish twice)
self.scene_publisher.publish(co)
rospy.sleep(0.5)
self.scene_publisher.publish(co)
rospy.sleep(0.5)
def __make_cylinder(name, pose, height, radius):
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]
return co
def __make_box(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [pose.pose]
return co
def __make_box(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [pose.pose]
return co
def __make_sphere(self, name, pose, radius):
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]
return co
def __make_sphere(self, name, pose, radius):
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]
return co
def __make_cylinder(name, pose, height, radius):
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]
return co
def convert_to_collision_object(link, full_linkname):
collision_object = link_to_collision_object(link, full_linkname)
if not collision_object:
return
link_root = get_root_collision_model(link).root_link.name
if link_root not in collision_objects:
collision_objects[link_root] = CollisionObject()
collision_objects[link_root].id = link_root
collision_objects[link_root].operation = CollisionObject.ADD
append_to_collision_object(collision_objects[link_root], collision_object)
def __make_cylinder(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cyl = SolidPrimitive()
cyl.type = SolidPrimitive.CYLINDER
cyl.dimensions = list(size)
co.primitives = [cyl]
co.primitive_poses = [pose.pose]
return co
def find_block(self):
"""
Find block and add it to the scene
"""
# Get necessary values
while not self.object_location:
print "Waiting for object location..."
rospy.sleep(1.0)
object_location = self.object_location
image_size = self.left_camera_size
while not self.current_pose:
print "Waiting for current pose values..."
rospy.sleep(1.0)
current_pose = self.current_pose.position
endeffector_height = current_pose.z - self.table_height
# Store values for calculation
Pp = [object_location.x, object_location.y, object_location.z]
Cp = [image_size[0] / 2, image_size[1] / 2, 0]
Bp = [current_pose.x, current_pose.y, current_pose.z]
Go = [-0.0230, 0.0110, 0.1100]
cc = [0.0021, 0.0021, 0.0000]
d = [endeffector_height, endeffector_height, 0.0000]
# Calculate block's position in workspace
# workspace = (Pp - Cp) * cc * d + Bp + Go
pixel_difference = map(operator.sub, Pp, Cp)
camera_constant = map(operator.mul, cc, d)
pixel_2_real = map(operator.mul, pixel_difference, camera_constant)
pixel_2_real[2] = endeffector_height * -1
workspace_without_gripper = map(operator.add, pixel_2_real, Bp)
workspace = map(operator.add, workspace_without_gripper, Go)
# Add block to scene
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = workspace_without_gripper[0]
p.pose.position.y = workspace_without_gripper[1]
p.pose.position.z = workspace_without_gripper[2] + 0.06
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "block"
co.header = p.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list((0.1, 0.1, 0.1))
co.primitives = [box]
co.primitive_poses = [p.pose]
pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True)
pub_co.publish(co)
return
def add_plane(self, name, pose, normal = (0, 0, 1), offset = 0):
""" Add a plane to the planning scene """
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
p = Plane()
p.coef = list(normal)
p.coef.append(offset)
co.planes = [p]
co.plane_poses = [pose.pose]
self._pub_co.publish(co)
def removeCollisionObject(self, name, use_service=True):
""" Remove an object. """
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = self._fixed_frame
o.id = name
o.operation = o.REMOVE
try:
del self._objects[name]
self._removed[name] = o
except KeyError:
pass
self.sendUpdate(o, None, use_service)
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 removeCollisionObject(self, name, wait=True):
""" Remove an object. """
o = CollisionObject()
o.header.stamp = rospy.Time.now()
o.header.frame_id = self._fixed_frame
o.id = name
o.operation = o.REMOVE
try:
del self._objects[name]
self._removed[name] = o
except KeyError:
pass
self._pub.publish(o)
if wait:
self.waitForSync()
def insert_object(self, pose, dims, name):
""" Expects a PoseStamped to be input, along with a Point and string """
primitive = SolidPrimitive()
primitive.type = primitive.BOX
# TODO: this is a hack to make the buffer bigger.
# By default, it appears to be 10cm x 10cm x 0cm, but I'm not sure which dimension is which
primitive.dimensions = [dims.x, dims.y, dims.z]
add_object = CollisionObject()
add_object.id = name
add_object.header.frame_id = pose.header.frame_id
add_object.operation = add_object.ADD
add_object.primitives.append(primitive)
add_object.primitive_poses.append(pose.pose)
self.collision_pub.publish(add_object)
def __make_mesh(name, pose, filename, scale=(1, 1, 1)):
co = CollisionObject()
if pyassimp is False:
raise MoveItCommanderException(
"Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt")
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face.indices[0],
face.indices[1],
face.indices[2]]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def execute(self, ud):
rospy.logdebug("Entered 'GRASP_ARM_PLAN' state.")
collision_object = CollisionObject()
collision_object.header.frame_id = 'odom'
collision_object.id = 'coffee'
primitive = SolidPrimitive()
primitive.type = primitive.CYLINDER
primitive.dimensions.append(0.15)
primitive.dimensions.append(0.06)
collision_object.primitives.append(primitive)
collision_object.operation = collision_object.ADD
ta = PoseStamped()
ta.header.frame_id = 'odom'
ta.header.stamp = rospy.Time.now()
ta.pose.position.x = 0.843708
ta.pose.position.y = -0.0977909
ta.pose.position.z = 1.10764
ta.pose.orientation.x = -0.126284
ta.pose.orientation.y = -0.368197
ta.pose.orientation.z = -0.525026
ta.pose.orientation.w = 0.756856
collision_object.primitive_poses.append(ud.grasp_target_pose.pose)
try:
res = self.make_plan_srv(targetPose=ud.grasp_target_pose, jointPos=[], planningSpace=computePlanRequest.CARTESIAN_SPACE, collisionObject=collision_object)
except rospy.ServiceException:
rospy.logerr("Failed to connect to the planning service.")
return 'grasp_arm_error'
if res.planningResult == computePlanResponse.PLANNING_FAILURE:
rospy.logwarn("Arm planning failed. Moving the base is probably required to reach target pose.")
return 'grasp_arm_plan_failed'
else:
ud.grasp_arm_plan = res.trajectory
rospy.sleep(rospy.Duration(10))
return 'grasp_arm_plan_succeeded'
def __init__(self):
"""
Initialize class
"""
# Overall MoveIt initialization
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
# MoveIt scene initialization
self.scene = moveit_commander.PlanningSceneInterface()
self.table_height = -0.320
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0.8
p.pose.position.y = 0.025
p.pose.position.z = -0.6
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "table"
co.header = p.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list((0.75, 1.25, 0.55))
co.primitives = [box]
co.primitive_poses = [p.pose]
pub_co = rospy.Publisher("collision_object", CollisionObject, latch=True)
pub_co.publish(co)
# Moveit group initialization
self.group = moveit_commander.MoveGroupCommander("left_arm")
self.group.set_goal_position_tolerance(0.12)
self.group.set_goal_orientation_tolerance(0.10)
# Gripper initialization
self.left_gripper = baxter_interface.Gripper("left", CHECK_VERSION)
self.left_gripper.reboot()
self.left_gripper.calibrate()
# Camera initialization
self.left_camera = baxter_interface.CameraController("left_hand_camera")
self.right_camera = baxter_interface.CameraController("right_hand_camera")
self.head_camera = baxter_interface.CameraController("head_camera")
self.right_camera.close()
self.head_camera.close()
self.left_camera.open()
self.left_camera.resolution = [1280, 800]
self.left_camera_size = [800, 1280]
_camera_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self._on_camera)
# Open CV initialization
cv2.namedWindow("Original", cv2.WINDOW_NORMAL)
cv2.moveWindow("Original", 1990, 30)
cv2.namedWindow("Thresholded", cv2.WINDOW_NORMAL)
cv2.moveWindow("Thresholded", 3270, 30)
self.low_h = 165
self.high_h = 179
self.low_s = 70
self.high_s = 255
self.low_v = 60
self.high_v = 255
self.bridge = CvBridge()
# Object location initialization
self.object_location = None
# Subscribe to pose at all times
self.current_pose = None
_pose_sub = rospy.Subscriber("/robot/limb/left/endpoint_state", EndpointState, self.set_current_pose)
return
def hand_over():
rospy.init_node('moveit_node')
listener = tf.TransformListener()
#listener.waitForTransform('left_hand_camera', 'base')
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
#rospy.init_node('moveit_node')
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
#right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(10)
#right_arm.set_planner_id('RRTConnectkConfigDefault')
#right_arm.set_planning_time(10)
# Add a collison object
# Build the attached object
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
#shape.dimensions.resize(3)
# print(shape.dimensions)
shape.dimensions.append(0.6)
shape.dimensions.append(1.5)
shape.dimensions.append(0.6)
# print(shape)
# Create pose
obj_pose = Pose()
obj_pose.position.x = 0.665
obj_pose.position.y = 0.062
obj_pose.position.z = -0.7
obj_pose.orientation.x = 0
obj_pose.orientation.y = 0
obj_pose.orientation.z = 0
obj_pose.orientation.w = 1
# create collision object
cobj = CollisionObject()
cobj.primitives = shape
cobj.primitive_poses=obj_pose
cobj.header.frame_id='base'
cobj.id = 'Table'
# create collision object message
# ATTACHED_COLLISION_OBJECT.link_name = TCP_LINK_NAME
# ATTACHED_COLLISION_OBJECT.object = cobj
# ATTACHED_COLLISION_OBJECT.object.header.frame_id = TCP_LINK_NAME
# ATTACHED_COLLISION_OBJECT.touch_links.push_back("gripper_body")
# Attache it to the scene
cobj.operation = CollisionObject.ADD
attach_object_publisher = rospy.Publisher('collision_object',CollisionObject,queue_size = 10)
attach_object_publisher.publish(cobj)
rospy.sleep(1)
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
input=1
pile_1=[]
pile_2=[]
pile_3=[]
while input:
#Original pose ------------------------------------------------------
original = PoseStamped()
original.header.frame_id = "base"
#the pose the baxter will first turn to
original.pose.position.x = 0.505
original.pose.position.y = 0.487
original.pose.position.z = 0.515
original.pose.orientation.x = 0.505
original.pose.orientation.y = 0.487
original.pose.orientation.z = 0.515
original.pose.orientation.w = -0.493
left_arm.set_pose_target(original)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
left_plan = left_arm.plan()
raw_input('Press <Enter> to move the left arm to the original pose: ')
left_arm.execute(left_plan)
rospy.sleep(2)
# First goal pose to pick up ----------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
while not rospy.is_shutdown():
try:
marker = rospy.wait_for_message('ar_pose_marker',AlvarMarkers)
#rospy.sleep(1)
#print(marker)
ar_name = marker.markers[0].id
arTagName = 'ar_marker_' + str(ar_name)
print(arTagName)
break
except:
continue
#arTagName = 'ar_marker_' + str(ar_name)
# Set up your subscriber and define its callback
image_topic = "/cameras/left_hand_camera/image"
my_image_msg = rospy.wait_for_message(image_topic, Image)
print("Received an image!")
# Convert your ROS Image message to OpenCV2
cv2_img = bridge.imgmsg_to_cv2(my_image_msg, "bgr8")
# Save your OpenCV2 image as a jpeg
cv2.imwrite('camera_image'+str(ar_name)+'.png', cv2_img)
# Carry out the digit recognition alg
# zipcode = digitRecog('camera_image'+str(ar_name)+'.png')
# Display the original image on Baxter's screen
display_pub = rospy.Publisher('/robot/xdisplay',Image, latch=True)
display_pub.publish(my_image_msg)
rospy.sleep(1)
while not rospy.is_shutdown():
try:
(trans_marker_cam, rot_marker_cam) = listener.lookupTransform(arTagName,'left_hand_camera', rospy.Time())
(trans_cam_hand, rot_cam_hand) = listener.lookupTransform('left_hand_camera', 'left_hand', rospy.Time())
(trans_hand_base, rot_hand_base) = listener.lookupTransform('left_hand', 'base', rospy.Time())
break
except:
continue
# Find the transform between the AR tag and the base
g_marker_cam = quat_to_rbt(trans_marker_cam, rot_marker_cam)
g_cam_hand = quat_to_rbt(trans_cam_hand, rot_cam_hand)
g_hand_base = quat_to_rbt(trans_hand_base, rot_hand_base)
g_final = linalg.inv(np.dot(np.dot(g_marker_cam, g_cam_hand), g_hand_base))
# print('final')
# print(g_final)
#x, y, and z position
x_pos = "%.3f" % (g_final[0,3])
y_pos = "%.3f" % (g_final[1,3])
z_pos = "%.3f" % (g_final[2,3])
# print(x_pos)
# print(y_pos)
# print(z_pos)
goal_1.pose.position.x = float(x_pos)
goal_1.pose.position.y = float(y_pos) - 0.2
goal_1.pose.position.z = float(z_pos)
#Orientation as a quaternion
goal_1.pose.orientation.x = -0.689
goal_1.pose.orientation.y = -0.013
goal_1.pose.orientation.z = -0.035
goal_1.pose.orientation.w = 0.723
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ')
left_arm.execute(left_plan)
rospy.Subscriber('/robot/range/left_hand_range/state',Range, callback)
rospy.sleep(1)
# print(mail_range)
#Check using IR to see of the mail is close enough
incre = 0.01
while mail_range > 0.06 and incre < 0.05:
print('Too far from the mail. Trying again...')
rospy.Subscriber('/robot/range/left_hand_range/state',Range, callback)
rospy.sleep(1)
print(mail_range)
goal_1.pose.position.y = float(y_pos) - 0.1 + incre
incre += 0.01
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to get the mail: ')
left_arm.execute(left_plan)
# print(mail_range)
#Close the Gripper
raw_input('Press <Enter> to close the gripper')
left_gripper.close(timeout=60)
print('Gripper closed')
#TODO- generate the recognized image and publish it
# Check http://sdk.rethinkrobotics.com/wiki/Display_Image_-_Code_Walkthrough
# display_pub.sleep()
img = cv2.imread('withRecog'+str(ar_name)+'.png')
msg = CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
rec_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
rec_pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(1)
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
if ar_name in (1, 3, 12):
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0.6
goal_2.pose.position.z = -0.18
pile_1.insert(0,ar_name)
elif ar_name in (0,9,15,16):
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0.3
goal_2.pose.position.z = -0.18
pile_2.insert(0,ar_name)
else:
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0
goal_2.pose.position.z = -0.18
pile_3.insert(0,ar_name)
print('pile_1')
print(pile_1)
print('pile_2')
print(pile_2)
print('pile_3')
print(pile_3)
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Execute the plan
#raw_input('Press <Enter> to move the left arm to the zipcode region: ')
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_2)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to the zipcode region: ')
left_arm.execute(left_plan)
#Release the Gripper
raw_input('Press <Enter> to open the gripper')
left_gripper.open(block=True)
print('Gripper opened')
#reg_pub.sleep()
#Continue or stop
input=int(raw_input('Press 1 to start/continue moving, or 0 to stop moving '))
return (pile_1,pile_2,pile_3)
pub = rospy.Publisher("obstacle_distance/registerObstacle", CollisionObject, queue_size = 1)
while pub.get_num_connections() < 1:
if rospy.is_shutdown():
exit(0)
rospy.logwarn("Please create a subscriber to 'obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)")
time.sleep(1.0)
rospy.loginfo("Continue ...")
# Publish a simple sphere
x = CollisionObject()
x.id = "Funny Sphere"
x.header.frame_id = root_frame
x.operation = CollisionObject.ADD
#x.operation = CollisionObject.REMOVE
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions.append(0.1) # radius
x.primitives.append(sphere)
pose = Pose()
pose.position.x = 0.35
pose.position.y = -0.45
pose.position.z = 0.8
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
pose.orientation.z = 0.0;
pose.orientation.w = 1.0;
x.primitive_poses.append(pose)
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
