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 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 create_primitive_and_pose_from_sq(self, sq_params):
"""
create a primitive object from superquadric parameters (no taper or bending)
"""
primitive = SolidPrimitive()
# size values are half the real dimensions
size = sq_params[7:10]
shape = sq_params[10:12]
# pose
pose = Pose()
pose.position = Point(*sq_params[0:3])
pose.orientation = Quaternion(*sq_params[3:7])
# shape
bEdgy = [i < 0.7 for i in shape]
if not any(bEdgy) and np.std(size) / min(size) < 0.1:
primitive.type = SolidPrimitive.SPHERE
primitive.dimensions.append(np.mean(size))
elif not bEdgy[1] and abs(size[0] - size[1]) / min(size[0:2]) < 0.1:
primitive.type = SolidPrimitive.CYLINDER
primitive.dimensions.append(2 * size[2])
primitive.dimensions.append(np.mean(size[0:2]))
else:
primitive.type = SolidPrimitive.BOX
primitive.dimensions.append(2 * size[0])
primitive.dimensions.append(2 * size[1])
primitive.dimensions.append(2 * size[2])
return (primitive, pose)
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 add_invisible_collision_object(self):
co_box = CollisionObject()
co_box.header.frame_id = 'base_footprint'
co_box.id = 'invisible_box'
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box_height = 0.76
box.dimensions.append(0.80)
box.dimensions.append(1.60)
box.dimensions.append(box_height)
co_box.primitives.append(box)
box_pose = Pose()
box_pose.position.x = 0.80
box_pose.position.y = 0.0
box_pose.position.z = box_height / 2.0
box_pose.orientation.w = 1.0
co_box.primitive_poses.append(box_pose)
co_box.operation = CollisionObject.ADD
color = ObjectColor()
color.id = 'invisible_box'
color.color.g = 1.0
color.color.a = 0.15
ps = PlanningScene()
ps.world.collision_objects.append(co_box)
ps.object_colors.append(color)
ps.is_diff = True
try:
self.planning_scene_service(ps)
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
def _add_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 publishCO(self, points, id, is_table=False):
pts = np.asarray(points)
dx, dy, dz = np.max(pts, axis=0) - np.min(pts, axis=0)
max_x, max_y, max_z = np.max(pts, axis=0)
min_x, min_y, min_z = np.min(pts, axis=0)
collision_object = CollisionObject()
collision_object.id = id
collision_object.header = self.lastHeader
object_shape = SolidPrimitive()
object_shape.type = SolidPrimitive.BOX
object_shape.dimensions.append(dx)
object_shape.dimensions.append(dy)
shape_pose = Pose()
shape_pose.position.x = min_x + dx / 2
shape_pose.position.y = min_y + dy / 2
if is_table:
object_shape.dimensions.append(max_z)
shape_pose.position.z = max_z / 2
else:
object_shape.dimensions.append(dz)
shape_pose.position.z = min_z + dz / 2
shape_pose.orientation.w = 1.0
collision_object.primitives.append(object_shape)
collision_object.primitive_poses.append(shape_pose)
collision_object.operation = CollisionObject.ADD
self.pub_collision.publish(collision_object)
def 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 __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(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 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 add_padded_lab():
rospy.loginfo("Adding padded order bin")
poses = []
objects = []
o_b_pose = Pose()
o_b_pose.orientation = Quaternion(x=0, y=0, z=0, w=1)
o_b_pose.position = Point(x=0.5, y=0.2, z=0.22)
objects.append(
SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.43, 0.68, 0.46]))
poses.append(o_b_pose)
wiring_pose = Pose()
wiring_pose.orientation = Quaternion(x=0, y=0, z=0, w=1)
wiring_pose.position = Point(x=-.55, y=0, z=0.15)
objects.append(
SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[0.6, 0.7, 0.4]))
poses.append(wiring_pose)
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "padded_lab"
co.header.frame_id = "/base_link"
co.header.stamp = rospy.Time.now()
co.primitives = objects
co.primitive_poses = poses
scene._pub_co.publish(co)
def 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 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 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 __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 _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_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 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_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 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 __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 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 __make_cylinder(name, pose, height, radius):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions = [height, radius]
co.primitives = [cylinder]
co.primitive_poses = [pose.pose]
return co
def __make_box(self, name, pose, size):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = list(size)
co.primitives = [box]
co.primitive_poses = [pose.pose]
return co
def __make_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 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 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 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 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 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 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 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 __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('left_arm')
self.group.set_goal_position_tolerance(0.01)
self.group.set_max_velocity_scaling_factor(.2)
# IK Service
self.ik_srv = rospy.ServiceProxy('ExternalTools/left/PositionKinematicsNode/IKService', SolvePositionIK)
# gripper and limb init
self.gripper = baxter_interface.Gripper('left')
self.gripper.open()
self.limb = baxter_interface.Limb('left')
#################### TF stuff
self.tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length
tf_listener = tf2_ros.TransformListener(self.tf_buffer)
#################### Cup Subscriber
rospy.Subscriber('cup_center',geometry_msgs.msg.Point,self.cupcb)
self.cup_plan = True
################ Collision Object
table = CollisionObject()
primitive = SolidPrimitive()
primitive.type = primitive.BOX
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.stamp = rospy.Time.now()
box_pose.header.frame_id = 'tablelol'
box_pose.pose.position.x = 1.25
box_pose.pose.position.y = 0.0
box_pose.pose.position.z = 0.0
table.primitives.append(primitive)
table.primitive_poses.append(box_pose)
table.operation = table.ADD
self.scene.add_box('table',box_pose,size=(.077,.073,.122))
# start joint trajectory action server
# node = roslaunch.core.Node('baxter_interface','joint_trajectory_action_server.py','rsdk_position_w_id_joint_trajectory_action_server',output='screen')
# launch = roslaunch.scriptapi.ROSLaunch()
# launch.start()
# process = launch.launch(node)
# print process.is_alive()
# rospy.sleep(5)
self.wait_for_cup()
self.grab_pub = rospy.Publisher('cup_grabbed',Bool,queue_size=10)
self.grab_pub.publish(False)
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 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 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 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 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 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 execute(self, ud):
rospy.logdebug("Entered 'GRASP_ARM_PLAN' state.")
collision_object = CollisionObject()
collision_object.header.frame_id = 'odom'
collision_object.id = 'coffee'
primitive = SolidPrimitive()
primitive.type = primitive.CYLINDER
primitive.dimensions.append(0.15)
primitive.dimensions.append(0.06)
collision_object.primitives.append(primitive)
collision_object.operation = collision_object.ADD
ta = PoseStamped()
ta.header.frame_id = 'odom'
ta.header.stamp = rospy.Time.now()
ta.pose.position.x = 0.843708
ta.pose.position.y = -0.0977909
ta.pose.position.z = 1.10764
ta.pose.orientation.x = -0.126284
ta.pose.orientation.y = -0.368197
ta.pose.orientation.z = -0.525026
ta.pose.orientation.w = 0.756856
collision_object.primitive_poses.append(ud.grasp_target_pose.pose)
try:
res = self.make_plan_srv(targetPose=ud.grasp_target_pose, jointPos=[], planningSpace=computePlanRequest.CARTESIAN_SPACE, collisionObject=collision_object)
except rospy.ServiceException:
rospy.logerr("Failed to connect to the planning service.")
return 'grasp_arm_error'
if res.planningResult == computePlanResponse.PLANNING_FAILURE:
rospy.logwarn("Arm planning failed. Moving the base is probably required to reach target pose.")
return 'grasp_arm_plan_failed'
else:
ud.grasp_arm_plan = res.trajectory
rospy.sleep(rospy.Duration(10))
return 'grasp_arm_plan_succeeded'
def 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
def getGoalConstraints(self, frame = None, q = None, q_goal=None, timeout=2.0, mode = ModeJoints):
if frame == None and q_goal == None:
raise RuntimeError('Must provide either a goal frame or joint state!')
return (None, None)
if q is None:
raise RuntimeError('Must provide starting position!')
return (None, None)
if len(self.robot_ns) > 0:
srv = rospy.ServiceProxy(self.robot_ns + "/compute_ik", moveit_msgs.srv.GetPositionIK)
else:
srv = rospy.ServiceProxy("compute_ik", moveit_msgs.srv.GetPositionIK)
goal = Constraints()
if frame is not None:
joints = self.ik(pm.toMatrix(frame),q)
elif q_goal is not None:
joints = q_goal
if len(joints) is not len(self.joint_names):
rospy.logerr("Invalid goal position. Number of joints in goal is not the same as robot's dof")
return (None, None)
if mode == ModeJoints or q_goal is not None:
for i in range(0,len(self.joint_names)):
joint = JointConstraint()
joint.joint_name = self.joint_names[i]
joint.position = joints[i]
joint.tolerance_below = 1e-6
joint.tolerance_above = 1e-6
joint.weight = 1.0
goal.joint_constraints.append(joint)
else:
print 'Setting cartesian constraint'
# TODO: Try to fix this again. Something is wrong
cartesian_costraint = PositionConstraint()
cartesian_costraint.header.frame_id = 'base_link'
cartesian_costraint.link_name = self.joint_names[-1]
# cartesian_costraint.target_point_offset = frame.p
bounding_volume = BoundingVolume()
sphere_bounding = SolidPrimitive()
sphere_bounding.type = sphere_bounding.SPHERE;
# constrain position with sphere 1 mm around target
sphere_bounding.dimensions.append(0.5)
bounding_volume.primitives.append(sphere_bounding)
sphere_pose = Pose()
sphere_pose.position = frame.p
sphere_pose.orientation.w = 1.0
bounding_volume.primitive_poses.append(sphere_pose)
cartesian_costraint.constraint_region = bounding_volume
cartesian_costraint.weight = 1.0
goal.position_constraints.append(cartesian_costraint)
orientation_costraint = OrientationConstraint()
orientation_costraint.header.frame_id = 'base_link'
orientation_costraint.link_name = self.joint_names[-1]
orientation_costraint.orientation = frame.M.GetQuaternion()
orientation_costraint.absolute_x_axis_tolerance = 0.1
orientation_costraint.absolute_y_axis_tolerance = 0.1
orientation_costraint.absolute_z_axis_tolerance = 0.1
orientation_costraint.weight = 1.0
goal.orientation_constraints.append(orientation_costraint)
print 'Done'
return(None, goal)
while pub.get_num_connections() < 1:
if rospy.is_shutdown():
exit(0)
rospy.logwarn("Please create a subscriber to 'obstacle_distance/registerObstacle' topic (Type: moveit_msgs/CollisionObject)")
time.sleep(1.0)
rospy.loginfo("Continue ...")
# Publish a simple sphere
x = CollisionObject()
x.id = "Funny Sphere"
x.header.frame_id = root_frame
x.operation = CollisionObject.ADD
#x.operation = CollisionObject.REMOVE
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions.append(0.1) # radius
x.primitives.append(sphere)
pose = Pose()
pose.position.x = 0.35
pose.position.y = -0.45
pose.position.z = 0.8
pose.orientation.x = 0.0;
pose.orientation.y = 0.0;
pose.orientation.z = 0.0;
pose.orientation.w = 1.0;
x.primitive_poses.append(pose)
# pub.publish(x)
time.sleep(1.0)
def hand_over():
rospy.init_node('moveit_node')
listener = tf.TransformListener()
#listener.waitForTransform('left_hand_camera', 'base')
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
#rospy.init_node('moveit_node')
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
#right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(10)
#right_arm.set_planner_id('RRTConnectkConfigDefault')
#right_arm.set_planning_time(10)
# Add a collison object
# Build the attached object
shape = SolidPrimitive()
shape.type = SolidPrimitive.BOX
#shape.dimensions.resize(3)
# print(shape.dimensions)
shape.dimensions.append(0.6)
shape.dimensions.append(1.5)
shape.dimensions.append(0.6)
# print(shape)
# Create pose
obj_pose = Pose()
obj_pose.position.x = 0.665
obj_pose.position.y = 0.062
obj_pose.position.z = -0.7
obj_pose.orientation.x = 0
obj_pose.orientation.y = 0
obj_pose.orientation.z = 0
obj_pose.orientation.w = 1
# create collision object
cobj = CollisionObject()
cobj.primitives = shape
cobj.primitive_poses=obj_pose
cobj.header.frame_id='base'
cobj.id = 'Table'
# create collision object message
# ATTACHED_COLLISION_OBJECT.link_name = TCP_LINK_NAME
# ATTACHED_COLLISION_OBJECT.object = cobj
# ATTACHED_COLLISION_OBJECT.object.header.frame_id = TCP_LINK_NAME
# ATTACHED_COLLISION_OBJECT.touch_links.push_back("gripper_body")
# Attache it to the scene
cobj.operation = CollisionObject.ADD
attach_object_publisher = rospy.Publisher('collision_object',CollisionObject,queue_size = 10)
attach_object_publisher.publish(cobj)
rospy.sleep(1)
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
input=1
pile_1=[]
pile_2=[]
pile_3=[]
while input:
#Original pose ------------------------------------------------------
original = PoseStamped()
original.header.frame_id = "base"
#the pose the baxter will first turn to
original.pose.position.x = 0.505
original.pose.position.y = 0.487
original.pose.position.z = 0.515
original.pose.orientation.x = 0.505
original.pose.orientation.y = 0.487
original.pose.orientation.z = 0.515
original.pose.orientation.w = -0.493
left_arm.set_pose_target(original)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
left_plan = left_arm.plan()
raw_input('Press <Enter> to move the left arm to the original pose: ')
left_arm.execute(left_plan)
rospy.sleep(2)
# First goal pose to pick up ----------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
while not rospy.is_shutdown():
try:
marker = rospy.wait_for_message('ar_pose_marker',AlvarMarkers)
#rospy.sleep(1)
#print(marker)
ar_name = marker.markers[0].id
arTagName = 'ar_marker_' + str(ar_name)
print(arTagName)
break
except:
continue
#arTagName = 'ar_marker_' + str(ar_name)
# Set up your subscriber and define its callback
image_topic = "/cameras/left_hand_camera/image"
my_image_msg = rospy.wait_for_message(image_topic, Image)
print("Received an image!")
# Convert your ROS Image message to OpenCV2
cv2_img = bridge.imgmsg_to_cv2(my_image_msg, "bgr8")
# Save your OpenCV2 image as a jpeg
cv2.imwrite('camera_image'+str(ar_name)+'.png', cv2_img)
# Carry out the digit recognition alg
# zipcode = digitRecog('camera_image'+str(ar_name)+'.png')
# Display the original image on Baxter's screen
display_pub = rospy.Publisher('/robot/xdisplay',Image, latch=True)
display_pub.publish(my_image_msg)
rospy.sleep(1)
while not rospy.is_shutdown():
try:
(trans_marker_cam, rot_marker_cam) = listener.lookupTransform(arTagName,'left_hand_camera', rospy.Time())
(trans_cam_hand, rot_cam_hand) = listener.lookupTransform('left_hand_camera', 'left_hand', rospy.Time())
(trans_hand_base, rot_hand_base) = listener.lookupTransform('left_hand', 'base', rospy.Time())
break
except:
continue
# Find the transform between the AR tag and the base
g_marker_cam = quat_to_rbt(trans_marker_cam, rot_marker_cam)
g_cam_hand = quat_to_rbt(trans_cam_hand, rot_cam_hand)
g_hand_base = quat_to_rbt(trans_hand_base, rot_hand_base)
g_final = linalg.inv(np.dot(np.dot(g_marker_cam, g_cam_hand), g_hand_base))
# print('final')
# print(g_final)
#x, y, and z position
x_pos = "%.3f" % (g_final[0,3])
y_pos = "%.3f" % (g_final[1,3])
z_pos = "%.3f" % (g_final[2,3])
# print(x_pos)
# print(y_pos)
# print(z_pos)
goal_1.pose.position.x = float(x_pos)
goal_1.pose.position.y = float(y_pos) - 0.2
goal_1.pose.position.z = float(z_pos)
#Orientation as a quaternion
goal_1.pose.orientation.x = -0.689
goal_1.pose.orientation.y = -0.013
goal_1.pose.orientation.z = -0.035
goal_1.pose.orientation.w = 0.723
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ')
left_arm.execute(left_plan)
rospy.Subscriber('/robot/range/left_hand_range/state',Range, callback)
rospy.sleep(1)
# print(mail_range)
#Check using IR to see of the mail is close enough
incre = 0.01
while mail_range > 0.06 and incre < 0.05:
print('Too far from the mail. Trying again...')
rospy.Subscriber('/robot/range/left_hand_range/state',Range, callback)
rospy.sleep(1)
print(mail_range)
goal_1.pose.position.y = float(y_pos) - 0.1 + incre
incre += 0.01
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to get the mail: ')
left_arm.execute(left_plan)
# print(mail_range)
#Close the Gripper
raw_input('Press <Enter> to close the gripper')
left_gripper.close(timeout=60)
print('Gripper closed')
#TODO- generate the recognized image and publish it
# Check http://sdk.rethinkrobotics.com/wiki/Display_Image_-_Code_Walkthrough
# display_pub.sleep()
img = cv2.imread('withRecog'+str(ar_name)+'.png')
msg = CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
rec_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
rec_pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(1)
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
if ar_name in (1, 3, 12):
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0.6
goal_2.pose.position.z = -0.18
pile_1.insert(0,ar_name)
elif ar_name in (0,9,15,16):
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0.3
goal_2.pose.position.z = -0.18
pile_2.insert(0,ar_name)
else:
#x, y, and z position
goal_2.pose.position.x = 0.7
goal_2.pose.position.y = 0
goal_2.pose.position.z = -0.18
pile_3.insert(0,ar_name)
print('pile_1')
print(pile_1)
print('pile_2')
print(pile_2)
print('pile_3')
print(pile_3)
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Execute the plan
#raw_input('Press <Enter> to move the left arm to the zipcode region: ')
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_2)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to the zipcode region: ')
left_arm.execute(left_plan)
#Release the Gripper
raw_input('Press <Enter> to open the gripper')
left_gripper.open(block=True)
print('Gripper opened')
#reg_pub.sleep()
#Continue or stop
input=int(raw_input('Press 1 to start/continue moving, or 0 to stop moving '))
return (pile_1,pile_2,pile_3)
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
try:
get_program_srv = rospy.ServiceProxy('/art/db/program/get', getProgram)
resp = get_program_srv(id=0)
print resp
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return
rospy.wait_for_service('/art/db/object/store')
# self.objects_table.insert(dict(name="profile_3", model_url="blablabla", obj_id=16))
# self.objects_table.insert(dict(name="profile_2", model_url="blablabla", obj_id=16))
# self.objects_table.update(dict(name="profile_3", model_url="blablabla", obj_id=15), ['name'])
bb = SolidPrimitive()
bb.type = SolidPrimitive.BOX
bb.dimensions.append(0.1)
bb.dimensions.append(0.1)
bb.dimensions.append(0.1)
try:
store_object_srv = rospy.ServiceProxy('/art/db/object/store', storeObject)
resp = store_object_srv(obj_id=3, name="profile_2", model_url="", type="profile", bbox=bb)
resp = store_object_srv(obj_id=4, name="profile_3", model_url="", type="profile", bbox=bb)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return
rospy.wait_for_service('/art/db/object/get')