Esempio n. 1
0
    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)
Esempio n. 2
0
 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)