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 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 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 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 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 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 _add_payload_to_planning_scene_msg(self, payload):
msg=payload.payload_msg
co = CollisionObject()
co.id = payload.payload_msg.name
co.header.frame_id = msg.header.frame_id
payload.attached_link=msg.header.frame_id
urdf_root=self.urdf.get_root()
urdf_chain=self.urdf.get_chain(urdf_root, payload.attached_link, joints=True, links=False)
urdf_chain.reverse()
touch_links=[]
touch_root = None
for j_name in urdf_chain:
j=self.urdf.joint_map[j_name]
if j.type != "fixed":
break
touch_root=j.parent
def _touch_recurse(touch):
ret=[touch]
if touch in self.urdf.child_map:
for c_j,c in self.urdf.child_map[touch]:
if self.urdf.joint_map[c_j].type == "fixed":
ret.extend(_touch_recurse(c))
return ret
if touch_root is not None:
touch_links.extend(_touch_recurse(touch_root))
for i in xrange(len(msg.collision_geometry.mesh_resources)):
mesh_filename=urlparse.urlparse(resource_retriever.get_filename(msg.collision_geometry.mesh_resources[i])).path
mesh_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.mesh_poses[i]))
mesh_scale=msg.collision_geometry.mesh_scales[i]
mesh_scale = (mesh_scale.x, mesh_scale.y, mesh_scale.z)
mesh_msg = load_mesh_file_to_mesh_msg(mesh_filename, mesh_scale)
co.meshes.extend(mesh_msg)
co.mesh_poses.extend([mesh_pose]*len(mesh_msg))
for i in xrange(len(msg.collision_geometry.primitives)):
co.primitives.append(msg.collision_geometry.primitives[i])
primitive_pose = rox_msg.transform2pose_msg(rox_msg.msg2transform(msg.pose) * rox_msg.msg2transform(msg.collision_geometry.primitive_poses[i]))
co.primitive_poses.append(primitive_pose)
aco = AttachedCollisionObject()
aco.link_name = payload.attached_link
aco.object = co
aco.touch_links = touch_links
aco.weight = msg.inertia.m
#self._pub_aco.publish(aco)
return aco
def publish_attached(self, attached_obj, side):
attached_obj.operation = CollisionObject.ADD
msg = AttachedCollisionObject()
msg.object = attached_obj
msg.link_name = side + "_gripper"
touch_links = [
side + "_gripper", side + "_gripper_base", side + "_hand_camera",
side + "_hand_range", "octomap"
]
self.attached_pub.publish(msg)
def 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 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 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 attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
aco = AttachedCollisionObject()
if (pose is not None) and filename:
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self.__submit(aco, attach=True)
def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
aco = AttachedCollisionObject()
if pose!=None and not filename.empty():
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def 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 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 attach(self, selected_id_index = 0): grasping_group = 'end_effector' touch_links = self.robot.get_link_names(group=grasping_group) aco = AttachedCollisionObject() aco.object = self.co_list[selected_id_index] aco.link_name = 'ee_link' if len(touch_links) > 0: aco.touch_links = touch_links else: aco.touch_links = ['ee_link'] self.__submit(aco, attach=True) return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=4)
def 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 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 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 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 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, 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 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_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 removeAttachedObject(self, name, use_service=True):
'''
Removes an attached object.
:param name: name of the object to be removed
:param use_service: If true, update will be sent via apply service
'''
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)
