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)
