def link_to_collision_object(link, full_linkname):
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
root_collision_model = get_root_collision_model(link)
link_pose_in_root_frame = pysdf.homogeneous2pose_msg(
concatenate_matrices(link.pose_world,
inverse_matrix(root_collision_model.pose_world)))
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print("Element %s with geometry type %s not supported. Ignored." %
(full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(
float(val) for val in linkpart.geometry_data['scale'].split())
mesh_path = linkpart.geometry_data['uri'].replace(
'model://', pysdf.models_path)
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = link_pose_in_root_frame
make_mesh(collision_object, full_linkname, link_pose_stamped,
mesh_path, scale)
elif linkpart.geometry_type == 'box':
scale = tuple(
float(val) for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius'])
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple(
(2.0 * float(linkpart.geometry_data['radius']),
float(linkpart.geometry_data['length'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(link_pose_in_root_frame)
#print('CollisionObject for %s:\n%s' % (full_linkname, collision_object))
return collision_object
def link_to_collision_object(link, full_linkname):
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
linkparts = getattr(link, 'collisions')
if is_ignored(link.parent_model):
print("Ignoring link %s." % full_linkname)
return
collision_object = CollisionObject()
collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
root_collision_model = get_root_collision_model(link)
link_pose_in_root_frame = pysdf.homogeneous2pose_msg(concatenate_matrices(link.pose_world, inverse_matrix(root_collision_model.pose_world)))
for linkpart in linkparts:
if linkpart.geometry_type not in supported_geometry_types:
print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type))
continue
if linkpart.geometry_type == 'mesh':
scale = tuple(float(val) for val in linkpart.geometry_data['scale'].split())
for models_path in pysdf.models_paths:
test_mesh_path = linkpart.geometry_data['uri'].replace('model://', models_path)
if os.path.isfile(test_mesh_path):
mesh_path = test_mesh_path
break
if mesh_path:
link_pose_stamped = PoseStamped()
link_pose_stamped.pose = link_pose_in_root_frame
make_mesh(collision_object, full_linkname, link_pose_stamped, mesh_path, scale)
else:
print("ERROR: No mesh found for '%s' in element '%s'." % (linkpart.geometry_data['uri'], full_linkname))
elif linkpart.geometry_type == 'box':
scale = tuple(float(val) for val in linkpart.geometry_data['size'].split())
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = scale
collision_object.primitives.append(box)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'sphere':
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius'])
collision_object.primitives.append(sphere)
collision_object.primitive_poses.append(link_pose_in_root_frame)
elif linkpart.geometry_type == 'cylinder':
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = tuple((2.0 * float(linkpart.geometry_data['radius']), float(linkpart.geometry_data['length'])))
collision_object.primitives.append(cylinder)
collision_object.primitive_poses.append(link_pose_in_root_frame)
#print('CollisionObject for %s:\n%s' % (full_linkname, collision_object))
return collision_object
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_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 attachBox(self,
name,
size_x,
size_y,
size_z,
x,
y,
z,
link_name,
touch_links=None,
detach_posture=None,
weight=0.0,
wait=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z
p.orientation.w = 1.0
o = self.makeSolidPrimitive(name, s, p)
o.header.frame_id = link_name
a = self.makeAttached(link_name, o, touch_links, detach_posture,
weight)
self._attached_objects[name] = a
self._attached_pub.publish(a)
if wait:
self.waitForSync()
def attachBox(self,
name,
size_x,
size_y,
size_z,
x,
y,
z,
link_name,
touch_links=None,
detach_posture=None,
weight=0.0,
use_service=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z
p.orientation.w = 1.0
o = self.makeSolidPrimitive(name, s, p)
o.header.frame_id = link_name
a = self.makeAttached(link_name, o, touch_links, detach_posture,
weight)
self._attached_objects[name] = a
self.sendUpdate(None, a, use_service)
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 add_board_object():
# Some publisher
scene_diff_publisher = rospy.Publisher('planning_scene', PlanningScene, queue_size=1)
rospy.sleep(10.0)
# Create board object
board = CollisionObject()
board.header.frame_id = 'base'
board.id = 'board'
board_box = SolidPrimitive()
board_box.type = 1
# board_box.dimensions = [3.0, 4.0, 0.185]
board_box.dimensions = [DEPTH_BIAS*2, 4.0, 3.0]
board.primitives.append(board_box)
board_pose = Pose()
board_pose.position.x = transformed_message.pose.position.x
board_pose.position.y = transformed_message.pose.position.y
board_pose.position.z = transformed_message.pose.position.z
# board_pose.orientation.x = transformed_message.pose.orientation.x
board_pose.orientation.x = 0
# board_pose.orientation.y = transformed_message.pose.orientation.y
board_pose.orientation.y = 0
# board_pose.orientation.z = transformed_message.pose.orientation.z
board_pose.orientation.z = 0
# board_pose.orientation.w = transformed_message.pose.orientation.w
board_pose.orientation.w = 0
board.primitive_poses.append(board_pose)
scene = PlanningScene()
scene.world.collision_objects.append(board)
scene.is_diff = True
scene_diff_publisher.publish(scene)
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 pub_obj(self, obj_type, pos, dim):
m = Marker()
m.id = self.obj_types.index(obj_type)
m.ns = "simple_tabletop"
m.type = Marker.CUBE
m.pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1))
m.header = Header(0, rospy.Time(0), "base_link")
m.scale = Vector3(*dim)
m.color = self.colors[obj_type]
mtext = deepcopy(m)
mtext.type = Marker.TEXT_VIEW_FACING
mtext.id += 100
mtext.text = obj_type
M = MarkerArray([m, mtext])
self.pub.publish(M)
#moveit collision objects
co = CollisionObject()
co.header = deepcopy(m.header)
co.id = obj_type
obj_shape = SolidPrimitive()
obj_shape.type = SolidPrimitive.BOX
obj_shape.dimensions = list(dim)
co.primitives.append(obj_shape)
obj_pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1))
co.primitive_poses = [obj_pose]
self.pub_co.publish(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 _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 attachBox(self,
name,
size_x,
size_y,
size_z,
x,
y,
z,
link_name,
touch_links=None,
detach_posture=None,
weight=0.0,
wait=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z
p.orientation.w = 1.0
r = self.makeSolidPrimitive(name, s,
p) # first remove from environment
r.operation = r.REMOVE
o = self.makeSolidPrimitive(name, s, p)
o.header.frame_id = link_name
a = self.makeAttached(link_name, o, touch_links, detach_posture,
weight)
self._attached_objects[name] = a
self.sentUpdate(r, a, wait)
def __addBoxWithOrientation(self,
name,
size_x,
size_y,
size_z,
x,
y,
z,
roll,
pitch,
yaw,
wait=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
ps = PoseStamped()
ps.header.frame_id = self._scene._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
ps.pose.orientation.x = q[0]
ps.pose.orientation.y = q[1]
ps.pose.orientation.z = q[2]
ps.pose.orientation.w = q[3]
self._scene.addSolidPrimitive(name, s, ps.pose, wait)
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 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 make_box(self, name, header, pose, size, scale, actualize = '1'):
box_names = ['9', '10', '11', '12', '0', '1', '2', '3']
triangle_names = ['13', '14', '15', '6', '7', '8']
exagon_names = ['4', '5']
if (actualize == '1'):
self.co.operation = CollisionObject.APPEND
elif (actualize == '2'):
self.co.operation = CollisionObject.ADD
elif (actualize == '3'):
self.co.operation = CollisionObject.REMOVE
self.co.id = name
print('co.id: ', self.co.id)
self.co.header = header
box = SolidPrimitive()
if name in box_names:
box.type = SolidPrimitive.BOX
elif name in triangle_names:
box.type = SolidPrimitive.CONE
elif name in exagon_names:
box.type = SolidPrimitive.CYLINDER
size = [(x*scale) for x in size]
box.dimensions = list(size)
self.co.primitives = [box]
self.co.primitive_poses = [pose.pose]
return self.co
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 _make_box(self, name, size, pos, quat=None, frame_id=None):
sp = SolidPrimitive()
sp.type = SolidPrimitive.BOX
sp.dimensions = size
return self._add_command(name,
self._make_pose(pos, quat),
solid=sp,
frame_id=frame_id)
def addCylinder(self, name, height, radius, ps, use_service=True):
'''
Insert new cylinder into planning scene
'''
s = SolidPrimitive()
s.dimensions = [height, radius]
s.type = s.CYLINDER
self.addSolidPrimitive(name, s, ps, use_service)
def publish_shape(self):
# send vehicle shape (only publish once)
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
shape.dimensions = [self.carla_actor.bounding_box.extent.x * 2,
self.carla_actor.bounding_box.extent.y * 2,
self.carla_actor.bounding_box.extent.z * 2]
self.publish_message(self.get_topic_prefix() + "/shape", shape, True)
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 __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 create_sphere(name, pose, radius, frame_id='/world_frame'):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header.frame_id = frame_id
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose]
return co
def add_box(co, pose, size = (0, 0, 1), offset=(0,0,0)):
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives.append(box)
p = deepcopy(pose)
p.position.x += offset[0]
p.position.y += offset[1]
p.position.z += offset[2]
co.primitive_poses.append(p)
return co
def _create_collision_object_from_primitive(self, model_instance_name,
model_pose, model_type, size):
collision_object = self._create_collision_object_base(
model_instance_name)
primitive = SolidPrimitive()
if ModelStates.BOX == model_type:
primitive.type = SolidPrimitive.BOX
primitive.dimensions = [i * 2 for i in size]
elif ModelStates.CYLINDER == model_type:
primitive.type = SolidPrimitive.CYLINDER
primitive.dimensions = [size[1] * 2, size[0]]
elif ModelStates.SPHERE == model_type:
primitive.type = SolidPrimitive.SPHERE
primitive.dimensions = [size[0]]
else:
raise TypeError(
"Primitive type {} not supported".format(model_type))
collision_object.primitives.append(primitive)
collision_object.primitive_poses.append(model_pose)
return collision_object
def create_box(name, pose, size, frame_id='/world_frame'):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header.frame_id = frame_id
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [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_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_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_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 handle_sphere(node):
if node.tag != 'sphere':
raise ValueError
v = node.attrib
c = node.getchildren()
p = SolidPrimitive(type=SolidPrimitive.SPHERE)
p.dimensions = [0]
p.dimensions[SolidPrimitive.SPHERE_RADIUS] = float(v['sphere_radius'])
return p,XMLSceneParser.handle_pose(c[0])
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 handle_cone(node):
if node.tag != 'cone':
raise ValueError
v = node.attrib
c = node.getchildren()
p = SolidPrimitive(type=SolidPrimitive.CONE)
p.dimensions = [0,0]
p.dimensions[SolidPrimitive.CONE_RADIUS] = float(v['cone_radius'])
p.dimensions[SolidPrimitive.CONE_HEIGHT] = float(v['cone_height'])
return p,XMLSceneParser.handle_pose(c[0])
def addBox(self, name, size_x, size_y, size_z, x, y, z, use_service=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
ps = PoseStamped()
ps.header.frame_id = self._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
ps.pose.orientation.w = 1.0
self.addSolidPrimitive(name, s, ps.pose, use_service)
def addCylinder(self, name, height, radius, x, y, z, wait=True):
s = SolidPrimitive()
s.dimensions = [height, radius]
s.type = s.CYLINDER
ps = PoseStamped()
ps.header.frame_id = self._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
ps.pose.orientation.w = 1.0
self.addSolidPrimitive(name, s, ps.pose, wait)
def addCylinder(self, name, height, radius, x, y, z, use_service=True):
s = SolidPrimitive()
s.dimensions = [height, radius]
s.type = s.CYLINDER
ps = PoseStamped()
ps.header.frame_id = self._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
ps.pose.orientation.w = 1.0
self.addSolidPrimitive(name, s, ps.pose, use_service)
def addSphere(self, name, radius, x, y, z, use_service=True):
s = SolidPrimitive()
s.dimensions = [radius]
s.type = s.SPHERE
ps = PoseStamped()
ps.header.frame_id = self._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
ps.pose.orientation.w = 1.0
self.addSolidPrimitive(name, s, ps.pose, use_service)
def handle_box(node):
if node.tag != 'box':
raise ValueError
v = node.attrib
c = node.getchildren()
p = SolidPrimitive(type=SolidPrimitive.BOX)
p.dimensions = [0,0,0]
p.dimensions[SolidPrimitive.BOX_X] = float(v['box_x'])
p.dimensions[SolidPrimitive.BOX_Y] = float(v['box_y'])
p.dimensions[SolidPrimitive.BOX_Z] = float(v['box_z'])
return p,XMLSceneParser.handle_pose(c[0])
def addBox(self, name, size_x, size_y, size_z, x, y, z, wait=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
ps = PoseStamped()
ps.header.frame_id = self._fixed_frame
ps.pose.position.x = x
ps.pose.position.y = y
ps.pose.position.z = z
ps.pose.orientation.w = 1.0
self.addSolidPrimitive(name, s, ps.pose, wait)
def attachBox(self, name, size_x, size_y, size_z, x, y, z, link_name,
touch_links=None, detach_posture=None, weight=0.0,
use_service=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z
p.orientation.w = 1.0
o = self.makeSolidPrimitive(name, s, p)
o.header.frame_id = link_name
a = self.makeAttached(link_name, o, touch_links, detach_posture, weight)
self._attached_objects[name] = a
self.sendUpdate(None, a, use_service)
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 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 set_target_object(self, primitive_type, target_pose, dimensions):
primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4}
self.sought_object = Object()
primitive = SolidPrimitive()
primitive.type = primitive_types[primitive_type.upper()]
primitive.dimensions = dimensions
self.sought_object.primitives.append(primitive)
p = Pose()
p.position.x = target_pose[0]
p.position.y = target_pose[1]
p.position.z = target_pose[2]
quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5])
p.orientation.x = quaternion[0]
p.orientation.y = quaternion[1]
p.orientation.z = quaternion[2]
p.orientation.w = quaternion[3]
self.sought_object.primitive_poses.append(p)
def attachBox(self, name, size_x, size_y, size_z, x, y, z, link_name,
touch_links=None, detach_posture=None, weight=0.0,
wait=True):
s = SolidPrimitive()
s.dimensions = [size_x, size_y, size_z]
s.type = s.BOX
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z
p.orientation.w = 1.0
o = self.makeSolidPrimitive(name, s, p)
o.header.frame_id = link_name
a = self.makeAttached(link_name, o, touch_links, detach_posture, weight)
self._attached_objects[name] = a
self._attached_pub.publish(a)
if wait:
self.waitForSync()
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 moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
