def move_to_pose(self, x, y, theta): # This is a hack to get the right trajectory to pick up the can at the kitchen counter in the kitchen scenario. if x == 0.16 and y == -0.16: self.execCommand("move_to_pose", ["3.5", "1.5", str(theta)]) self.execCommand("move_to_pose", ["-0.45", "1.3", str(theta)]) self.execCommand("move_to_pose", ["-0.7", "-0.18", str(theta)]) self.execCommand("move_to_pose", ["-0.2", "-0.16", str(theta)]) self.execCommand("move_to_pose", ["0.12", "-0.16", str(theta)]) # This is a hack to get the right trajectory to release the can at the dining table in the kitchen scenario. if x == 1.7 and y == 5.8: self.execCommand("move_to_pose", ["0.23", "0.6", str(theta)]) self.execCommand("move_to_pose", ["0.5", "0.8", str(theta)]) self.execCommand("move_to_pose", ["1.2", "0.9", str(theta)]) self.execCommand("move_to_pose", ["3.5", "1.2", str(theta)]) self.execCommand("move_to_pose", ["3.5", "5.8", str(theta)]) destination = Pose() quaternion = RobotInterface.quaternion2DFromAngle(theta) destination.position.x = x destination.position.y = y destination.position.z = 0 destination.orientation.x = quaternion[0] destination.orientation.y = quaternion[1] destination.orientation.z = quaternion[2] destination.orientation.w = quaternion[3] self.moveTo3DPose(destination)
def move_to_pose(self, x, y, theta): destination = Pose() quaternion = RobotInterface.quaternion2DFromAngle(theta) destination.position.x = x destination.position.y = y destination.position.z = 0 destination.orientation.x = quaternion[0] destination.orientation.y = quaternion[1] destination.orientation.z = quaternion[2] destination.orientation.w = quaternion[3] self.walkTo3DPose(destination)