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'