class MoveArm(smach.State):
def __init__(self, *args, **kwargs):
smach.State.__init__(self, outcomes=["succeeded", "not_succeeded", "failed"])
self.mp = MotionPlan()
# move arm
self.mp += cob_arm_navigation_python.MoveArm.MoveArm(*args, **kwargs)
def execute(self, userdata):
sss.set_light("blue")
wi = WorldInterface()
wi.reset_attached_objects()
# add floor
floor_extent = [3.0, 3.0, 0.1]
floor_pose = conversions.create_pose_stamped(
[0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1], "/base_link"
) # magic numbers are cool
wi.add_collision_box(floor_pose, floor_extent, "floor")
if not self.mp.plan(5).success:
sss.set_light("green")
return "not_succeeded"
sss.set_light("yellow")
# run, handle errors
i = 0
for ex in self.mp.execute():
if not ex.wait(80.0).success:
sss.set_light("red")
return "failed"
i += 1
sss.set_light("green")
return "succeeded"
class MoveArm(smach.State):
def __init__(self, *args, **kwargs):
smach.State.__init__(self,
outcomes=['succeeded', 'not_succeeded', 'failed'])
self.mp = MotionPlan()
# move arm
self.mp += cob_arm_navigation_python.MoveArm.MoveArm(*args, **kwargs)
def execute(self, userdata):
sss.set_light('blue')
wi = WorldInterface()
wi.reset_attached_objects()
# add floor
floor_extent = [3.0, 3.0, 0.1]
floor_pose = conversions.create_pose_stamped(
[0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1],
'/base_link') # magic numbers are cool
wi.add_collision_box(floor_pose, floor_extent, "floor")
if not self.mp.plan(5).success:
sss.set_light('green')
return "not_succeeded"
sss.set_light('yellow')
# run, handle errors
i = 0
for ex in self.mp.execute():
if not ex.wait(80.0).success:
sss.set_light('red')
return 'failed'
i += 1
sss.set_light('green')
return 'succeeded'
def __init__(self, *args, **kwargs):
smach.State.__init__(self,
outcomes=['succeeded', 'not_succeeded', 'failed'])
self.mp = MotionPlan()
# move arm
self.mp += cob_arm_navigation_python.MoveArm.MoveArmUnplanned(
*args, **kwargs)
def execute(self, userdata, overwrite_presets=dict()):
presets = self.default_presets
presets.update(overwrite_presets)
sss.set_light('blue')
wi = WorldInterface()
wi.reset_attached_objects()
graspdata = dict()
print userdata.object
print "time difference = ", (
rospy.Time.now() - userdata.object.pose.header.stamp).to_sec()
# add wall
wall_extent = [3.0, 0.1, 2.5]
wall_pose = conversions.create_pose_stamped(
[0, -0.99 - wall_extent[1], wall_extent[2] / 2.0, 0, 0, 0, 1],
'/base_link') # magic numbers are cool
wi.add_collision_box(wall_pose, wall_extent, "wall")
# add floor
floor_extent = [3.0, 3.0, 0.1]
floor_pose = conversions.create_pose_stamped(
[0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1],
'/base_link') # magic numbers are cool
wi.add_collision_box(floor_pose, floor_extent, "floor")
#transform into base_link
grasp_pose = transform_listener.transform_pose_stamped(
'/base_link', userdata.object.pose, use_most_recent=False)
print grasp_pose
# add object bounding box
obj_pose = deepcopy(userdata.object.pose)
lwh = userdata.object.bounding_box_lwh
m1 = pm.toMatrix(pm.fromMsg(obj_pose.pose))
m2 = pm.toMatrix(pm.fromTf(((0, 0, lwh.z / 2.0), (0, 0, 0, 1))))
obj_pose.pose = pm.toMsg(pm.fromMatrix(numpy.dot(m1, m2)))
wi.add_collision_box(obj_pose, (lwh.x * 2.0, lwh.y * 2.0, lwh.z),
"grasp_object")
# compute minimal height of object bounding box edges in base_link
min_z = grasp_pose.pose.position.z
for k in [(0.5, 0.5, 1.0), (0.5, 0.5, 0.0), (0.5, -0.5, 1.0),
(0.5, -0.5, 0.0), (-0.5, 0.5, 1.0), (-0.5, 0.5, 0.0),
(-0.5, -0.5, 1.0), (-0.5, -0.5, 0.0)]:
m1 = pm.toMatrix(pm.fromMsg(grasp_pose.pose)) # BFromO
m2 = pm.toMatrix(
pm.fromTf(((k[0] * lwh.x, k[1] * lwh.y, k[2] * lwh.z),
(0, 0, 0, 1)))) # inO
min_z = min(min_z,
pm.toMsg(pm.fromMatrix(numpy.dot(
m1, m2))).position.z) #min_z inB
# add table
table_extent = (2.0, 2.0, min_z)
# base_link
# x - points towards front of robot
# y - follow right hand rule
# z - points upwards
table_pose = conversions.create_pose_stamped([
-0.5 - table_extent[0] / 2.0, 0, table_extent[2] / 2.0, 0, 0, 0, 1
], '/base_link') # center of table, 0.5 meter behind the robot
wi.add_collision_box(table_pose, table_extent, "table")
# calculate grasp and lift pose
grasp_pose.pose.position.x += presets['grasp_offset'][0]
grasp_pose.pose.position.y += presets['grasp_offset'][1]
grasp_pose.pose.position.z += presets['grasp_offset'][2]
grasp_pose.pose.orientation = Quaternion(
*quaternion_from_euler(*presets['fixed_orientation_euler'])
) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal
graspdata['height'] = grasp_pose.pose.position.z - table_extent[2]
pregrasp_pose = deepcopy(grasp_pose)
pregrasp_pose.pose.position.x += presets['pregrasp_offset'][0]
pregrasp_pose.pose.position.y += presets['pregrasp_offset'][1]
pregrasp_pose.pose.position.z += presets['pregrasp_offset'][2]
lift_pose = deepcopy(grasp_pose)
lift_pose.pose.position.x += presets['lift_offset'][0]
lift_pose.pose.position.y += presets['lift_offset'][1]
lift_pose.pose.position.z += presets['lift_offset'][2]
mp = MotionPlan()
# open hand
mp += CallFunction(sss.move, 'sdh', 'cylopen', False)
mp += MoveArm('arm', [pregrasp_pose, [presets['tcp_link']]],
seed=presets['pregrasp_seed'])
mp += MoveComponent('sdh', 'cylopen', True)
# allow collison hand/object
for l in hand_description.HandDescription('arm').touch_links:
mp += EnableCollision("grasp_object", l)
# goto grasp
mp += MoveArm('arm', [grasp_pose, [presets['tcp_link']]
]) # Move sdh_grasp_link to grasp_pose
# close hand
mp += MoveComponent('sdh', 'cylclosed')
# check grasp
#mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data)
# disable collison
mp += ResetCollisions()
# attach object
mp += AttachObject('arm', "grasp_object")
# enable collision
mp += EnableCollision("grasp_object", "table")
# lift motion
mp += MoveArm('arm', [lift_pose, [presets['tcp_link']]
]) # Move sdh_grasp_link to lift_pose
# disable collison
mp += ResetCollisions()
# move away
#mp += MoveArm('arm', [pregrasp_pose,[presets['tcp_link']]])
# goto hold
mp += MoveArm('arm', 'hold')
userdata.graspdata = graspdata
if not mp.plan(5).success:
sss.set_light('green')
return "not_grasped"
sss.set_light('yellow')
sss.say(["I am grasping " + userdata.object.label + " now."], False)
# run, handle errors
i = 0
for ex in mp.execute():
if not ex.wait(80.0).success:
sss.set_light('red')
return 'failed'
i += 1
sss.set_light('green')
return 'grasped'
def execute(self, userdata):
sss.set_light("blue")
wi = WorldInterface()
wi.reset_attached_objects()
graspdata = dict()
print userdata.object
# add wall
wall_extent = [3.0, 0.1, 2.5]
wall_pose = conversions.create_pose_stamped(
[0, -0.99 - wall_extent[1], wall_extent[2] / 2.0, 0, 0, 0, 1], "base_link"
) # magic numbers are cool
wi.add_collision_box(wall_pose, wall_extent, "wall")
# add floor
floor_extent = [3.0, 3.0, 0.1]
floor_pose = conversions.create_pose_stamped(
[0, 0, floor_extent[2] / 2.0, 0, 0, 0, 1], "/base_link"
) # magic numbers are cool
wi.add_collision_box(floor_pose, floor_extent, "floor")
# transform into base_link
grasp_pose = transform_listener.transform_pose_stamped("base_link", userdata.object.pose, use_most_recent=False)
# add object bounding box
obj_pose = deepcopy(grasp_pose)
lwh = userdata.object.bounding_box_lwh
m1 = pm.toMatrix(pm.fromMsg(obj_pose.pose))
m2 = pm.toMatrix(pm.fromTf(((0, 0, lwh.z / 2.0), (0, 0, 0, 1))))
obj_pose.pose = pm.toMsg(pm.fromMatrix(numpy.dot(m1, m2)))
wi.add_collision_box(obj_pose, (lwh.x * 2.0, lwh.y * 2.0, lwh.z), "grasp_object")
print grasp_pose
# add table
table_extent = (2.0, 2.0, grasp_pose.pose.position.z)
table_pose = conversions.create_pose_stamped(
[-0.5 - table_extent[0] / 2.0, 0, table_extent[2] / 2.0, 0, 0, 0, 1], "base_link"
)
wi.add_collision_box(table_pose, table_extent, "table")
# calculate grasp and lift pose
grasp_pose.pose.position.x += 0.02
grasp_pose.pose.position.y += 0.02
grasp_pose.pose.position.z += 0.1 # 0.03 + 0.05
grasp_pose.pose.orientation = Quaternion(
*quaternion_from_euler(-1.706, 0.113, 2.278)
) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal
graspdata["height"] = grasp_pose.pose.position.z - table_extent[2]
pregrasp_pose = deepcopy(grasp_pose)
pregrasp_pose.pose.position.x += 0.20
pregrasp_pose.pose.position.y += 0.11
pregrasp_pose.pose.position.z += 0.1
lift_pose = deepcopy(grasp_pose)
lift_pose.pose.position.z += 0.03
mp = MotionPlan()
# open hand
mp += CallFunction(sss.move, "sdh", "cylopen", False)
mp += MoveArm("arm", [pregrasp_pose, ["sdh_grasp_link"]], seed="pregrasp")
mp += MoveComponent("sdh", "cylopen", True)
# allow collison hand/object
# for l in hand_description.HandDescription('arm').touch_links:
# mp += EnableCollision("grasp_object", l)
#
# disable collison
# mp += ResetCollisions()
# goto grasp
mp += MoveArmUnplanned("arm", [grasp_pose, ["sdh_grasp_link"]])
# close hand
mp += MoveComponent("sdh", "cylclosed")
# check grasp
# mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data)
# attach object
mp += AttachObject("arm", "grasp_object")
# enable collision
mp += EnableCollision("grasp_object", "table")
# lift motion
mp += MoveArmUnplanned("arm", [lift_pose, ["sdh_grasp_link"]])
# disable collison
mp += ResetCollisions()
# move away
mp += MoveArm("arm", [pregrasp_pose, ["sdh_grasp_link"]])
# goto hold
mp += MoveArm("arm", "hold")
userdata.graspdata = graspdata
if not mp.plan(5).success:
sss.set_light("green")
return "not_grasped"
sss.set_light("yellow")
sss.say(["I am grasping " + userdata.object.label + " now."], False)
# run, handle errors
i = 0
for ex in mp.execute():
if not ex.wait(80.0).success:
sss.set_light("red")
return "failed"
i += 1
sss.set_light("green")
return "grasped"
grasp_pose.pose = wi.collision_object('milk').poses[0]
p = grasp_pose.pose.position
#p.x, p.y, p.z = [-0.8,-0.2,0.9]
p.x += 0.02
p.y += 0.02
p.z += 0.02
o = grasp_pose.pose.orientation
o.x,o.y,o.z,o.w = quaternion_from_euler(-1.581, -0.019, 2.379)
lift_pose = deepcopy(grasp_pose)
p = lift_pose.pose.position
p.x += 0.0
p.y += -0.05
p.z += 0.03
mp = MotionPlan()
#mp += MoveArm('arm', 'pregrasp')
mp += MoveComponent('tray', 'down')
mp += MoveComponent('sdh', 'cylopen')
mp += MoveArm("arm",[grasp_pose,['sdh_grasp_link']])
mp += MoveComponent('sdh', 'cylclosed')
mp += AttachObject('arm', 'milk')
mp += EnableCollision('milk','table_ikea')
mp += MoveArm("arm",[lift_pose,['sdh_grasp_link']])
mp += ResetCollisions()
#mp += MoveArm('arm', 'home')
#mp += MoveComponent('tray', 'up')
#mp += MoveArm('arm', 'hold')
mp += MoveArm('arm', [['base_link',[0.64-0.161, -0.11+0.056,0.832+0.3],[-1.441, 0.118, -0.078]],['sdh_grasp_link']])
mp += MoveComponent('tray', 'up')
def execute(self, userdata, overwrite_presets = dict()):
presets = self.default_presets
presets.update(overwrite_presets)
sss.set_light('blue')
wi = WorldInterface()
wi.reset_attached_objects()
graspdata = dict()
print userdata.object
print "time difference = ", (rospy.Time.now() - userdata.object.pose.header.stamp).to_sec()
# add wall
wall_extent = [3.0,0.1,2.5]
wall_pose = conversions.create_pose_stamped([ 0, -0.99 - wall_extent[1], wall_extent[2]/2.0 ,0,0,0,1], '/base_link') # magic numbers are cool
wi.add_collision_box(wall_pose, wall_extent, "wall")
# add floor
floor_extent = [3.0,3.0,0.1]
floor_pose = conversions.create_pose_stamped([ 0, 0, floor_extent[2]/2.0 ,0,0,0,1], '/base_link') # magic numbers are cool
wi.add_collision_box(floor_pose, floor_extent, "floor")
#transform into base_link
grasp_pose = transform_listener.transform_pose_stamped('/base_link', userdata.object.pose, use_most_recent=False)
print grasp_pose
# add object bounding box
obj_pose = deepcopy(userdata.object.pose)
lwh = userdata.object.bounding_box_lwh
m1 = pm.toMatrix( pm.fromMsg(obj_pose.pose) )
m2 = pm.toMatrix( pm.fromTf( ((0,0, lwh.z/2.0),(0,0,0,1)) ) )
obj_pose.pose = pm.toMsg( pm.fromMatrix(numpy.dot(m1,m2)) )
wi.add_collision_box(obj_pose,(lwh.x*2.0,lwh.y*2.0,lwh.z) , "grasp_object")
# compute minimal height of object bounding box edges in base_link
min_z = grasp_pose.pose.position.z
for k in [(0.5,0.5,1.0),(0.5,0.5,0.0),(0.5,-0.5,1.0),(0.5,-0.5,0.0),(-0.5,0.5,1.0),(-0.5,0.5,0.0),(-0.5,-0.5,1.0),(-0.5,-0.5,0.0)]:
m1 = pm.toMatrix( pm.fromMsg(grasp_pose.pose) ) # BFromO
m2 = pm.toMatrix( pm.fromTf( ((k[0]*lwh.x,k[1]*lwh.y, k[2]*lwh.z),(0,0,0,1)) ) ) # inO
min_z = min(min_z,pm.toMsg( pm.fromMatrix(numpy.dot(m1,m2))).position.z) #min_z inB
# add table
table_extent = (2.0, 2.0, min_z)
# base_link
# x - points towards front of robot
# y - follow right hand rule
# z - points upwards
table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], '/base_link') # center of table, 0.5 meter behind the robot
wi.add_collision_box(table_pose, table_extent, "table")
# calculate grasp and lift pose
grasp_pose.pose.position.x += presets['grasp_offset'][0]
grasp_pose.pose.position.y += presets['grasp_offset'][1]
grasp_pose.pose.position.z += presets['grasp_offset'][2]
grasp_pose.pose.orientation = Quaternion(*quaternion_from_euler(*presets['fixed_orientation_euler'])) # orientation of sdh_grasp_link in base_link for 'grasp' joint goal
graspdata['height'] = grasp_pose.pose.position.z - table_extent[2]
pregrasp_pose = deepcopy(grasp_pose)
pregrasp_pose.pose.position.x += presets['pregrasp_offset'][0]
pregrasp_pose.pose.position.y += presets['pregrasp_offset'][1]
pregrasp_pose.pose.position.z += presets['pregrasp_offset'][2]
lift_pose = deepcopy(grasp_pose)
lift_pose.pose.position.x += presets['lift_offset'][0]
lift_pose.pose.position.y += presets['lift_offset'][1]
lift_pose.pose.position.z += presets['lift_offset'][2]
mp = MotionPlan()
# open hand
mp += CallFunction(sss.move, 'sdh','cylopen', False)
mp += MoveArm('arm',[pregrasp_pose,[presets['tcp_link']]], seed = presets['pregrasp_seed'])
mp += MoveComponent('sdh','cylopen', True)
# allow collison hand/object
for l in hand_description.HandDescription('arm').touch_links:
mp += EnableCollision("grasp_object", l)
# goto grasp
mp += MoveArm('arm', [grasp_pose,[presets['tcp_link']]]) # Move sdh_grasp_link to grasp_pose
# close hand
mp += MoveComponent('sdh','cylclosed')
# check grasp
#mp += CheckService('/sdh_controller/is_cylindric_grasped', Trigger, lambda res: res.success.data)
# disable collison
mp += ResetCollisions()
# attach object
mp += AttachObject('arm', "grasp_object")
# enable collision
mp += EnableCollision("grasp_object", "table")
# lift motion
mp += MoveArm('arm', [lift_pose,[presets['tcp_link']]]) # Move sdh_grasp_link to lift_pose
# disable collison
mp += ResetCollisions()
# move away
#mp += MoveArm('arm', [pregrasp_pose,[presets['tcp_link']]])
# goto hold
mp += MoveArm('arm', 'hold')
userdata.graspdata = graspdata
if not mp.plan(5).success:
sss.set_light('green')
return "not_grasped"
sss.set_light('yellow')
sss.say(["I am grasping " + userdata.object.label + " now."],False)
# run, handle errors
i = 0
for ex in mp.execute():
if not ex.wait(80.0).success:
sss.set_light('red')
return 'failed'
i+=1
sss.set_light('green')
return 'grasped'
def __init__(self, *args, **kwargs):
smach.State.__init__(self, outcomes=["succeeded", "not_succeeded", "failed"])
self.mp = MotionPlan()
# move arm
self.mp += cob_arm_navigation_python.MoveArm.MoveArm(*args, **kwargs)
grasp_pose.pose = wi.collision_object('milk').poses[0]
p = grasp_pose.pose.position
#p.x, p.y, p.z = [-0.8,-0.2,0.9]
p.x += 0.02
p.y += 0.02
p.z += 0.02
o = grasp_pose.pose.orientation
o.x, o.y, o.z, o.w = quaternion_from_euler(-1.581, -0.019, 2.379)
lift_pose = deepcopy(grasp_pose)
p = lift_pose.pose.position
p.x += 0.0
p.y += -0.05
p.z += 0.03
mp = MotionPlan()
#mp += MoveArm('arm', 'pregrasp')
mp += MoveComponent('tray', 'down')
mp += MoveComponent('sdh', 'cylopen')
mp += MoveArm("arm", [grasp_pose, ['sdh_grasp_link']])
mp += MoveComponent('sdh', 'cylclosed')
mp += AttachObject('arm', 'milk')
mp += EnableCollision('milk', 'table_ikea')
mp += MoveArm("arm", [lift_pose, ['sdh_grasp_link']])
mp += ResetCollisions()
#mp += MoveArm('arm', 'home')
#mp += MoveComponent('tray', 'up')
#mp += MoveArm('arm', 'hold')
mp += MoveArm('arm', [[
'base_link', [0.64 - 0.161, -0.11 + 0.056, 0.832 + 0.3],
