def execute(self, component_name, target=PoseStamped(), blocking=True):
ah = ActionHandle("move_arm_cartesian_planned", component_name, target, blocking)
rospy.loginfo("Move <<%s>> CARTESIAN PLANNED", component_name)
mp = MotionPlan()
mp += MoveArm("arm",[target,['sdh_grasp_link']])
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
else:
rospy.logerr("Planning failed")
ah.set_failed(4)
return ah
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
def execute(self, component_name, target="", blocking=True):
ah = ActionHandle("move_arm_joint_planned", component_name, target, blocking)
rospy.loginfo("Move planned <<%s>> to <<%s>>",component_name,target)
# get joint values from parameter server
if type(target) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target)
else:
param = target
# check trajectory parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
last_point = param[-1]
if type(last_point) is str:
if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + last_point):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/" + component_name + "/" + last_point)
ah.set_failed(2)
return ah
last_point = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + last_point)
last_point = last_point[0] # \todo TODO: hack because only first point is used, no support for trajectories inside trajectories
#print last_point
elif type(last_point) is list:
rospy.logdebug("last_point is a list")
else:
rospy.logerr("no valid parameter for %s: not a list of lists or strings, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
# here: last_point should be list of floats/ints
#print last_point
if not len(last_point) == self.DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...",component_name,self.DOF,len(last_point))
print "parameter is:",param
ah.set_failed(3)
return ah
for value in last_point:
#print value,"type2 = ", type(value)
if not ((type(value) is float) or (type(value) is int)): # check type
#print type(value)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...",component_name)
print "parameter is:",param
ah.set_failed(3)
return ah
rospy.logdebug("accepted value %f for %s",value,component_name)
mp = MotionPlan()
mp += MoveArm("arm", [last_point])
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(3)
break
else:
rospy.logerr("Planning failed")
ah.set_failed(3)
return ah
