def execute(self, userdata):
action_cmdr.move_gripper("open")
action_cmdr.move_arm("zeroposition")
for object in userdata.object_list:
sss.move_arm("zeroposition")
##object.pose.pose.position.z = object.pose.pose.position.z + 0.02
#object.pose.pose.position.x = object.pose.pose.position.x + 0.01
#object.pose.pose.position.y = object.pose.pose.position.y - 0.005
#object.transform.transform.translation.z = object.transform.transform.translation.z + 0.02
#object.transform.transform.translation.x = object.transform.transform.translation.x + 0.01
#object.transform.transform.translation.y = object.transform.transform.translation.y - 0.005
#handle_arm = sss.move_arm([object.pose.pose.position.x, object.pose.pose.position.y, object.pose.pose.position.z, "/base_link"])
object_pose = PoseStamped()
object_pose.pose.position.x = object.transform.transform.x
object_pose.pose.position.y = object.transform.transform.y
object_pose.pose.position.z = object.transform.transform.z
object_pose.pose.orientation = object.transform.tranform.rotation
object_pose.header.frame_id = object.transform.header.frame_id
handle_arm = sss.move_arm(object_pose)
if handle_arm.get_state() == 3:
sss.move_gripper("close")
rospy.sleep(3.0)
sss.move_arm("zeroposition")
return 'succeeded'
else:
rospy.logerr('could not find IK for current object')
return 'failed'
def execute(self, userdata):
action_cmdr.move_gripper("open")
action_cmdr.move_arm("zeroposition")
for object in userdata.object_list:
sss.move_arm("zeroposition")
##object.pose.pose.position.z = object.pose.pose.position.z + 0.02
#object.pose.pose.position.x = object.pose.pose.position.x + 0.01
#object.pose.pose.position.y = object.pose.pose.position.y - 0.005
#object.transform.transform.translation.z = object.transform.transform.translation.z + 0.02
#object.transform.transform.translation.x = object.transform.transform.translation.x + 0.01
#object.transform.transform.translation.y = object.transform.transform.translation.y - 0.005
#handle_arm = sss.move_arm([object.pose.pose.position.x, object.pose.pose.position.y, object.pose.pose.position.z, "/base_link"])
object_pose = PoseStamped()
object_pose.pose.position.x = object.transform.transform.x
object_pose.pose.position.y = object.transform.transform.y
object_pose.pose.position.z = object.transform.transform.z
object_pose.pose.orientation = object.transform.tranform.rotation
object_pose.header.frame_id = object.transform.header.frame_id
handle_arm = sss.move_arm(object_pose)
if handle_arm.get_state() == 3:
sss.move_gripper("close")
rospy.sleep(3.0)
sss.move_arm("zeroposition")
return 'succeeded'
else:
rospy.logerr('could not find IK for current object')
return 'failed'
def execute(self, blocking=True):
ah = action_cmdr.move_tray(target="up", blocking=True)
if ah.get_error_code() != 0:
return ah
ah = action_cmdr.move_arm("look_at_table")
if ah.get_error_code() != 0:
return ah
ah = action_cmdr.move_head(target="back", blocking=True)
if ah.get_error_code() != 0:
return ah
return action_cmdr.move_torso(target="home", blocking=True)
def execute(self, userdata):
# init arm
arm_to_init = action_cmdr.move_arm("look_at_table")
#init gripper
# gripper_open = action_cmdr.move_gripper("open")
rospy.loginfo("robot initialized")
return 'succeeded'
def execute(self, userdata):
# init arm
arm_to_init = action_cmdr.move_arm("look_at_table")
#init gripper
# gripper_open = action_cmdr.move_gripper("open")
rospy.loginfo("robot initialized")
return 'succeeded'
