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 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 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 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 publish_parsed_urdf(parsed_urdf, scene, ap, attach=False):
""" Publish link geometry for every joint's child. """
for name, joint in parsed_urdf["joints"].items():
# get the child link data
link = parsed_urdf["links"][joint["child"]]
# publish the child links collision geometry
if link["type"] == "box":
if attach:
scene.attach_box("table", joint["child"], joint["pose"],
link["size"], ["table"])
else:
scene.add_box(joint["child"], joint["pose"], link["size"])
elif link["type"] == "cylinder":
if attach:
aco = AttachedCollisionObject()
aco.touch_links = ["table"]
aco.object = make_cylinder(joint["child"], joint["pose"],
link["length"], link["radius"])
aco.link_name = "table"
print(aco.object)
ap.publish(aco)
else:
scene.add_cylinder(joint["child"], joint["pose"],
link["length"], link["radius"])
else:
scene.add_mesh(joint["child"], joint["pose"], link["filename"],
link["scale"])
def virtualObjectLeftHand():
print "Creating a virtual object attached to gripper..."
# for more details refer to ROS docs for moveit_msgs/AttachedCollisionObject
object1 = AttachedCollisionObject()
object1.link_name = "left_HandGripLink"
object1.object.header.frame_id = "left_HandGripLink"
object1.object.id = "object1"
object1_prim = SolidPrimitive()
object1_prim.type = SolidPrimitive.CYLINDER
object1_prim.dimensions=[None, None] # set initial size of the list to 2
object1_prim.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = 0.23
object1_prim.dimensions[SolidPrimitive.CYLINDER_RADIUS] = 0.06
object1_pose = pm.toMsg(PyKDL.Frame(PyKDL.Rotation.RotY(math.pi/2)))
object1.object.primitives.append(object1_prim)
object1.object.primitive_poses.append(object1_pose)
object1.object.operation = CollisionObject.ADD
object1.touch_links = ['left_HandPalmLink',
'left_HandFingerOneKnuckleOneLink',
'left_HandFingerOneKnuckleTwoLink',
'left_HandFingerOneKnuckleThreeLink',
'left_HandFingerTwoKnuckleOneLink',
'left_HandFingerTwoKnuckleTwoLink',
'left_HandFingerTwoKnuckleThreeLink',
'left_HandFingerThreeKnuckleTwoLink',
'left_HandFingerThreeKnuckleThreeLink']
return object1
def 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 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 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 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_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
if len(touch_links) > 0:
aco.touch_links = touch_links
else:
aco.touch_links = [link]
self._pub_aco.publish(aco)
def remove_attached_object(self, link, name=''):
"""
Remove object from planning scene
"""
aco = AttachedCollisionObject()
aco.object.operation = CollisionObject.REMOVE
aco.link_name = link
aco.object.id = name
self._pub_aco.publish(aco)
def 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 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(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_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_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
aco = AttachedCollisionObject()
if (pose is not None) and filename:
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self.__submit(aco, attach=True)
def makeAttached(self, link_name, obj, touch_links, detach_posture,
weight):
o = AttachedCollisionObject()
o.link_name = link_name
o.object = obj
if touch_links:
o.touch_links = touch_links
if detach_posture:
o.detach_posture = detach_posture
o.weight = weight
return o
def attach_mesh(self, link, name, pose=None, filename='', size=(1, 1, 1), touch_links=[]):
aco = AttachedCollisionObject()
if (pose is not None) and filename:
aco.object = self.__make_mesh(name, pose, filename, size)
else:
aco.object = self.__make_existing(name)
aco.link_name = link
aco.touch_links = [link]
if len(touch_links) > 0:
aco.touch_links = touch_links
self.__submit(aco, attach=True)
def attach_box(self, link, name, pose=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 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 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 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_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 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 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 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 table():
'''
Create the table and back plate that is connected to the robot base.
@return An AttachedCollisionObject representing the table and back plate
'''
HEIGHT_OFFSET = 0.004
table = AttachedCollisionObject()
table.link_name = "base_link"
table.object.header.frame_id = "/world"
table.object.id = "table"
tablePose = Pose()
tablePose.position.x -= 0.5
tablePose.position.z = -0.1 / 2 - HEIGHT_OFFSET #- 0.26
tablePose.orientation.w = 1.0
tableBox = SolidPrimitive()
tableBox.type = tableBox.BOX
tableBox.dimensions = [1.5, 1, 0.1]
table.object.primitives.append(tableBox)
table.object.primitive_poses.append(tablePose)
backpanelPose = Pose()
backpanelPose.position.y = -0.5
backpanelPose.position.z = 0.5 - HEIGHT_OFFSET - 0.26
backpanelPose.orientation.w = 1.0
backpanelBox = SolidPrimitive()
backpanelBox.type = backpanelBox.BOX
backpanelBox.dimensions = [1.5, 0.1, 1]
table.object.primitives.append(backpanelBox)
table.object.primitive_poses.append(backpanelPose)
table.object.operation = table.object.ADD
return table
def 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 roof():
'''
Create a roof that constrains the random movements of the robot.
@return An AttachedCollisionObject representing the roof
'''
roof = AttachedCollisionObject()
roof.link_name = "base_link"
roof.object.header.frame_id = "world"
roof.object.id = "roof"
roofPose = Pose()
roofPose.position.z = .6
roofPose.orientation.w = 1.0
roofBox = SolidPrimitive()
roofBox.type = roofBox.BOX
roofBox.dimensions = [1.5, 1, 0.025]
roof.object.primitives.append(roofBox)
roof.object.primitive_poses.append(roofPose)
return roof
print "Moving to the current position..."
js_start = velma.getLastJointState()
velma.moveJoint(js_start[1],
1.0,
start_time=0.5,
position_tol=15.0 / 180.0 * math.pi)
error = velma.waitForJoint()
if error != 0:
print "The action should have ended without error, but the error code is", error
exitError(4)
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] = 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',
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)
