def attach_mesh(self, link, name, pose, filename, touch_links = []):
aco = AttachedCollisionObject()
aco.object = self.__make_mesh(name, pose, filename)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_mesh(self, link, name, pose, filename, touch_links=[]):
aco = AttachedCollisionObject()
aco.object = self.__make_mesh(name, pose, filename)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def 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_box(self, link, name, pose, size = (1, 1, 1), touch_links = []):
aco = AttachedCollisionObject()
aco.object = self.__make_box(name, pose, size)
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
self._pub_aco.publish(aco)
def 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 is not None) and filename:
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self.__submit(aco, attach=True)
def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
aco = AttachedCollisionObject()
if pose!=None and not filename.empty():
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_mesh(self, link, name, pose = None, filename = '', size = (1, 1, 1), touch_links = []):
aco = AttachedCollisionObject()
if pose!=None and not filename.empty():
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_mesh_custom(self, link, name, mesh, pose=None, touch_links=[]):
aco = AttachedCollisionObject()
if pose != None and mesh:
aco.object = self.__make_mesh_custom(name, pose, mesh, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self._pub_aco.publish(aco)
def attach_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 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 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 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 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 _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 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_sphere(link, name, pose, radius, touch_links=[]):
aco = AttachedCollisionObject()
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [radius]
co.primitives = [sphere]
co.primitive_poses = [pose.pose]
aco.object = co
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
scene._pub_aco.publish(aco)
def 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_cylinder(link, name, pose, height, radius, touch_links=[]):
aco = AttachedCollisionObject()
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
aco.object = co
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
scene._pub_aco.publish(aco)
def attachCylinder(self,link,name,pose,size,touch_links=[]):
"""
Attaches a cylinder to a gripper
:param name: Name of the new cylinder object
:type name: str
:param pose: Pose of the new object
:type pose: geometry_msgs.msg.PoseStamped
:param size: Dimensions of the new object (height[m],radius[m])
:type size: (float, float)
:param touch_links: links that are already in collision with the attached object (added links do not trigger a collision)
:type touch_links: list(str)
"""
#print "attach object with name", name
#print "touch links",touch_links
aco = AttachedCollisionObject()
aco.object = self.__make_cylinder(name, pose, size)
aco.link_name = link
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
self.scene._pub_aco.publish(aco)
def 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 attach_object(self, group_name):
rospy.loginfo('attaching object to ' + group_name)
# select toolframe depending on group name
tool_frame = TOOL_FRAME_RIGHT if 'right' in group_name else TOOL_FRAME_LEFT
# pose for attached object in tool frame coordiantes
pose = Pose()
pose.position.z = 0.05
pose.orientation.w = 1.0
primitive = SolidPrimitive()
primitive.type = primitive.BOX
primitive.dimensions = [0.07, 0.07, 0.07]
o = CollisionObject()
o.header.frame_id = tool_frame
o.id = "handed_object"
o.primitives = [primitive]
o.primitive_poses = [pose]
o.operation = o.ADD
a = AttachedCollisionObject()
a.object = o
# allow collisions with hand links
a.touch_links = ALLOWED_TOUCH_LINKS
# attach object to tool frame
a.link_name = tool_frame
# don't delete old planning scene, if we didn't get one so far, create a new one
scene = self.current_planning_scene if self.current_planning_scene is not None else PlanningScene(
)
# add attached object to scene
scene.robot_state.attached_collision_objects.append(a)
# mark scene as changed
scene.is_diff = True
self.pub_ps.publish(scene)
def attach_object(self, group_name):
rospy.loginfo('attaching object to ' + group_name)
# select toolframe depending on group name
tool_frame = TOOL_FRAME_RIGHT if 'right' in group_name else TOOL_FRAME_LEFT
# pose for attached object in tool frame coordiantes
pose = Pose()
pose.position.z = 0.05
pose.orientation.w = 1.0
primitive = SolidPrimitive()
primitive.type = primitive.BOX
primitive.dimensions = [0.07, 0.07, 0.07]
o = CollisionObject()
o.header.frame_id = tool_frame
o.id = "handed_object"
o.primitives = [primitive]
o.primitive_poses = [pose]
o.operation = o.ADD
a = AttachedCollisionObject()
a.object = o
# allow collisions with hand links
a.touch_links = ALLOWED_TOUCH_LINKS
# attach object to tool frame
a.link_name = tool_frame
# don't delete old planning scene, if we didn't get one so far, create a new one
scene = self.current_planning_scene if self.current_planning_scene is not None else PlanningScene()
# add attached object to scene
scene.robot_state.attached_collision_objects.append(a)
# mark scene as changed
scene.is_diff = True
self.pub_ps.publish(scene)
def planAndExecuteAttached(q_map):
"""!
Fukcja planuje oraz wykonuje ruch do zadanej pozycji stawow z uwzglednieniem przenoszonego obiektu
@param q_dest slownik: Slownik {nazwa_stawu:pozycja_docelowa} zawierajacy docelowe pozycje stawow.
"""
print "Creating a virtual object attached to gripper..."
# for more details refer to ROS docs for moveit_msgs/AttachedCollisionObject
object1 = AttachedCollisionObject()
object1.link_name = "right_HandGripLink"
object1.object.header.frame_id = "right_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 = [
'right_HandPalmLink', 'right_HandFingerOneKnuckleOneLink',
'right_HandFingerOneKnuckleTwoLink',
'right_HandFingerOneKnuckleThreeLink',
'right_HandFingerTwoKnuckleOneLink',
'right_HandFingerTwoKnuckleTwoLink',
'right_HandFingerTwoKnuckleThreeLink',
'right_HandFingerThreeKnuckleTwoLink',
'right_HandFingerThreeKnuckleThreeLink'
]
# print "Publishing the attached object marker on topic /attached_objects"
# pub = MarkerPublisherThread(object1)
# pub.start()
print "Moving to valid position, using planned trajectory."
goal_constraint_1 = qMapToConstraints(
q_map, 0.01, group=velma.getJointGroup("impedance_joints"))
for i in range(20):
rospy.sleep(0.5)
js = velma.getLastJointState()
print "Planning (try", i, ")..."
traj = p.plan(js[1], [goal_constraint_1],
"impedance_joints",
max_velocity_scaling_factor=0.15,
max_acceleration_scaling_factor=0.2,
planner_id="RRTConnect",
attached_collision_objects=[object1])
if traj == None:
continue
print "Executing trajectory..."
if not velma.moveJointTraj(
traj, start_time=0.5, position_tol=20.0 / 180.0 * math.pi):
exitError(5)
if velma.waitForJoint() == 0:
break
else:
print "The trajectory could not be completed, retrying..."
continue
rospy.sleep(0.5)
js = velma.getLastJointState()
if not isConfigurationClose(q_map, js[1]):
print "Nie udalo sie zaplanowac ruchu do pozycji docelowej, puszka nie zostala odlozona."
exitError(6)
object1.object.header.frame_id = "right_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] = 1.0
object1_prim.dimensions[SolidPrimitive.CYLINDER_RADIUS] = 0.02
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 = [
'right_HandPalmLink', 'right_HandFingerOneKnuckleOneLink',
'right_HandFingerOneKnuckleTwoLink',
'right_HandFingerOneKnuckleThreeLink',
'right_HandFingerTwoKnuckleOneLink',
'right_HandFingerTwoKnuckleTwoLink',
'right_HandFingerTwoKnuckleThreeLink',
'right_HandFingerThreeKnuckleTwoLink',
'right_HandFingerThreeKnuckleThreeLink'
]
print "Publishing the attached object marker on topic /attached_objects"
pub = MarkerPublisherThread(object1)
pub.start()
q_map_goal = {
'torso_0_joint': 0,
'right_arm_0_joint': -0.3,
'right_arm_1_joint': -1.8,
'right_arm_2_joint': -1.25,
'right_arm_3_joint': 1.57,