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 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 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"
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, target=PoseStamped(), blocking=True):
ah = ActionHandle("pick_up", "dummy", "", blocking)
rospy.loginfo("Picking up object...")
#ah = self.actions.grasp_object(target, blocking)
wi = WorldInterface()
wi.reset_attached_objects()
wi.reset_collision_objects()
# add table
table_extent = (2.0, 2.0, 1.0)
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")
mp = MotionPlan()
mp += CallFunction(sss.move, 'torso','front')
#mp += MoveArm('arm',['pregrasp'])
mp += CallFunction(sss.move, 'sdh','cylopen', False)
# OpenIssues:
# - where to place the sdh_grasp_link to grasp the object located at target?
# - there is no orientation in the target? -> hardcoded taken from pregrasp
grasp_pose = PoseStamped()
grasp_pose = deepcopy(target)
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.orientation.x = 0.220
grasp_pose.pose.orientation.y = 0.670
grasp_pose.pose.orientation.z = -0.663
grasp_pose.pose.orientation.w = -0.253
mp += MoveArm('arm',[grasp_pose,['sdh_grasp_link']], seed = 'pregrasp')
mp += CallFunction(sss.move, 'sdh','cylclosed', True)
#ah = self.actions.lift_object(target, blocking)
lift_pose = PoseStamped()
lift_pose = deepcopy(target)
lift_pose.header.stamp = rospy.Time.now()
lift_pose.pose.position.z += 0.08
lift_pose.pose.orientation.x = 0.220
lift_pose.pose.orientation.y = 0.670
lift_pose.pose.orientation.z = -0.663
lift_pose.pose.orientation.w = -0.253
mp += MoveArm('arm',[lift_pose,['sdh_grasp_link']])
#ah = self.actions.retrieve_object(target, blocking)
#mp += MoveArm('arm',['pregrasp'])
mp += MoveArm('arm',['wavein'])
planning_res = mp.plan(2)
print planning_res
if planning_res.success:
for e in mp.execute():
exec_res = e.wait()
print exec_res
if not exec_res.success:
#rospy.logerr("Execution of MotionExecutable %s failed", e.name)
ah.set_failed(4)
break
ah.set_succeeded()
else:
rospy.logerr("Planning failed")
ah.set_failed(4)
return ah
