Example #1
0
def main(robotIP, PORT=9559):

	motionProxy = ALProxy("ALMotion", robotIP, PORT)
	postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

	motionProxy.wakeUp()
	postureProxy.goToPosture("StandInit", 0.5)
	audioProxy = ALProxy("ALAudioDevice", robotIP, PORT)
	audioProxy.setOutputVolume(45)

	time.sleep(2)
	motionProxy.setFallManagerEnabled(False)
	

	t = Thread(target=CoMBalancing)

	t.setDaemon(True)
	t.start()

	initArmPose("129.125.178.114", 9559)	### Could disturb balance

	runtime = time.time()
	useSensorValues = True
	result = motionProxy.getRobotPosition(useSensorValues)
	print "Robot Position", result


	# Example showing how to use this information to know the robot's diplacement.
	initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
	endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
	# Compute robot's' displacement
	robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
	vector = robotMove.toVector()
	### Use while loop and check for robotPosition
	while abs(vector[0]) < 1.5:
		i = 0
		print i
		step(robotIP)
		endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
		# Compute robot's' displacement
		robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
		vector = robotMove.toVector()
		

	#for i in range(6):
	#	step(robotIP)

	runtime = time.time() - runtime
	data = {'xCom': xList, 'b1_custom': accList, 'b2_custom': polyList, 'b1': accList1, 'b2': polyList1, 'acc': forceList, 'time': yList, 'runtime': runtime, 'distance': vector[0]}

	### trial_default_X  (With box, no disturbance)
	### 10 trials for default
	### trial_disturbed_x (With box, disturbed)
	### 0 trial for disturbed
	with open('trial_disturbed_4', 'wb') as f:
	 	pickle.dump(data, f)

	print "Time taken: ", runtime, " (s)"
	motionProxy.rest()
def main(robotIP, PORT=9559):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = motionProxy.getRobotPosition(useSensorValues)
    print "Robot Position", result

    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = False
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))

    # Make the robot move
    motionProxy.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))

    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
    print "Robot Move:", robotMove

    # Go to rest position
    motionProxy.rest()
Example #3
0
def main(robotIP, PORT=9559):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    result = motionProxy.getNextRobotPosition()
    print "Next Robot Position", result

    # Example showing how to use this information to know the robot's diplacement
    # during the move process.

    # Make the robot move
    motionProxy.post.moveTo(0.6, 0.0, 0.5)  # No blocking due to post called
    time.sleep(1.0)
    initRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())

    # Make the robot move
    motionProxy.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())

    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
    print "Robot Move :", robotMove

    # Go to rest position
    motionProxy.rest()
Example #4
0
def main(robotIP, PORT=9559):
    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send NAO to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    result = motionProxy.getNextRobotPosition()
    print "Next Robot Position", result

    # Example showing how to use this information to know the robot's diplacement
    # during the move process.

    # Make the robot move
    motionProxy.post.moveTo(0.6, 0.0, 0.5) # No blocking due to post called
    time.sleep(1.0)
    initRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())

    # Make the robot move
    motionProxy.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = almath.Pose2D(motionProxy.getNextRobotPosition())

    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
    print "Robot Move :", robotMove

    # Go to rest position
    motionProxy.rest()
Example #5
0
def main():


    motionProxy = getProxy(Proxies.Motion)
    postureProxy = getProxy(Proxies.RobotPosture)

    # --- Defaults ----
    speed = 0.5
    useSensorValues = False

    # Stand Init
    postureProxy.goToPosture("StandInit", speed)

    # Get position in the world
    result = motionProxy.getRobotPosition(False)
    print "Robot Postition", result

    # Get robot displacement
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    print "Init Robot Position", initRobotPosition

    motionProxy.moveTo(0.5, 0.0, radians(280))

    endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))
    print "End Position", endRobotPosition

    robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
    print "Robot move", robotMove
    def TakeClosePic(self):
        # x – normalized, unitless, velocity along X-axis.
        # +1 and -1 correspond to the maximum velocity in the forward and backward directions, respectively.
        x = 1.0
        y = 0.0
        theta = 0.0
        # index of image
        image_i = 0
        frequency = 0.1
        # get current time
        t0 = time.time()
        initRobotPosition = almath.Pose2D(
            self.motion_service.getRobotPosition(False))

        #  since now the robot is 1.5 away from the human
        #  let the robot move 0.7 meters forward while taking photos(now the velocity is 0.1 meter per second)
        #  in order to keep 0.8 meter distance away from human

        while ((time.time() - t0) <= 7.0):
            # once 7s has been passed, break the loop
            if (time.time() - t0) > 7.0:
                break
            # print "time difference :" + str((time.time() - t0))
            self.motion_service.moveToward(x, y, theta,
                                           [["MaxVelXY", frequency]])
            image_i += 1
            self.GetImage(image_i)

        # stop the robot
        self.motion_service.stopMove()
        endRobotPosition = almath.Pose2D(
            self.motion_service.getRobotPosition(False))
        robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
        robotMove.theta = almath.modulo2PI(robotMove.theta)
        print "Distance change and angle change after approaching the human:", robotMove
    def NaviageToTarget(self):

        # navigation_service = self.session.service("ALNavigation")
        try:
            Xmove = self.Xlast
            # if the distance (along x-axis) that the robot needs to move not equal to 0
            if (Xmove != None):
                initRobotPosition = almath.Pose2D(
                    self.motion_service.getRobotPosition(False))
                # navigation_service.navigateTo(Xmove,0)
                print(
                    "Distance (along x-axis) that the robot needs to move \n" +
                    "(in meter) to approach a human:", Xmove)
                self.motion_service.moveTo(Xmove, 0, 0, _async=True)
                self.motion_service.waitUntilMoveIsFinished()
                time.sleep(0.5)
                endRobotPosition = almath.Pose2D(
                    self.motion_service.getRobotPosition(False))
                robotMove = almath.pose2DInverse(
                    initRobotPosition) * endRobotPosition
                print(
                    "Distance change and angle change after moveTo() function:",
                    robotMove)

        except KeyboardInterrupt:
            print "Interrupted by user, stopping moving"
            #stop
            self.motion_service.stopMove()
            sys.exit(0)
Example #8
0
def main(session):
    """
    Move To: Small example to make Nao Move To an Objective.
    """
    # Get the services ALMotion & ALRobotPosture.

    motion_service = session.service("ALMotion")
    posture_service = session.service("ALRobotPosture")

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Stand Init
    posture_service.goToPosture("StandInit", 0.5)

    #####################
    ## Enable arms control by move algorithm
    #####################
    motion_service.setMoveArmsEnabled(True, True)
    #~ motion_service.setMoveArmsEnabled(False, False)

    #####################
    ## FOOT CONTACT PROTECTION
    #####################
    #~ motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
    motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    #####################
    ## get robot position before move
    #####################
    initRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))

    X = 0.3
    Y = 0.1
    Theta = math.pi/2.0
    motion_service.moveTo(X, Y, Theta, _async=True)
    # wait is useful because with _async moveTo is not blocking function
    motion_service.waitUntilMoveIsFinished()

    #####################
    ## get robot position after move
    #####################
    endRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))

    #####################
    ## compute and print the robot motion
    #####################
    robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
    # return an angle between ]-PI, PI]
    robotMove.theta = almath.modulo2PI(robotMove.theta)
    print "Robot Move:", robotMove

    # Go to rest position
    motion_service.rest()
Example #9
0
File: move.py Project: ajnirp/mirch
def main(session):
    """
    Move To: Small example to make Nao Move To an Objective.
    """
    # Get the services ALMotion & ALRobotPosture.

    motion_service = session.service("ALMotion")
    posture_service = session.service("ALRobotPosture")

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Stand Init
    posture_service.goToPosture("StandInit", 0.5)

    #####################
    ## Enable arms control by move algorithm
    #####################
    motion_service.setMoveArmsEnabled(True, True)
    #~ motion_service.setMoveArmsEnabled(False, False)

    #####################
    ## FOOT CONTACT PROTECTION
    #####################
    #~ motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
    motion_service.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    #####################
    ## get robot position before move
    #####################
    initRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))

    X = 0.3
    Y = 0.1
    Theta = math.pi/2.0
    motion_service.moveTo(X, Y, Theta, _async=True)
    # wait is useful because with _async moveTo is not blocking function
    motion_service.waitUntilMoveIsFinished()

    #####################
    ## get robot position after move
    #####################
    endRobotPosition = almath.Pose2D(motion_service.getRobotPosition(False))

    #####################
    ## compute and print the robot motion
    #####################
    robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
    # return an angle between ]-PI, PI]
    robotMove.theta = almath.modulo2PI(robotMove.theta)
    print "Robot Move:", robotMove

    # Go to rest position
    motion_service.rest()
def main(robotIP, PORT=9559):

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    #####################
    ## Enable arms control by move algorithm
    #####################
    motionProxy.setMoveArmsEnabled(True, True)
    #~ motionProxy.setMoveArmsEnabled(False, False)

    #####################
    ## FOOT CONTACT PROTECTION
    #####################
    #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    #####################
    ## get robot position before move
    #####################
    initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    X = 0.0
    Y = 0.0
    Theta = math.pi/2.0
    motionProxy.post.moveTo(X, Y, Theta)
    motionProxy.setAngles("HeadYaw", 0.6, 0.6)
    # wait is useful because with post moveTo is not blocking function
    motionProxy.waitUntilMoveIsFinished()

    #####################
    ## get robot position after move
    #####################
    endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    #####################
    ## compute and print the robot motion
    #####################
    robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition
    # return an angle between ]-PI, PI]
    robotMove.theta = m.modulo2PI(robotMove.theta)
    print "Robot Move:", robotMove

    # Go to rest position
    motionProxy.rest()
Example #11
0
def main(robotIP, PORT=9559):

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    #####################
    ## Enable arms control by move algorithm
    #####################
    motionProxy.setMoveArmsEnabled(True, True)
    #~ motionProxy.setMoveArmsEnabled(False, False)

    #####################
    ## FOOT CONTACT PROTECTION
    #####################
    #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]])
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    #####################
    ## get robot position before move
    #####################
    initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    X = 0.3
    Y = 0.1
    Theta = math.pi / 2.0
    motionProxy.post.moveTo(X, Y, Theta)
    # wait is useful because with post moveTo is not blocking function
    motionProxy.waitUntilMoveIsFinished()

    #####################
    ## get robot position after move
    #####################
    endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    #####################
    ## compute and print the robot motion
    #####################
    robotMove = m.pose2DInverse(initRobotPosition) * endRobotPosition
    # return an angle between ]-PI, PI]
    robotMove.theta = m.modulo2PI(robotMove.theta)
    print "Robot Move:", robotMove

    # Go to rest position
    motionProxy.rest()
Example #12
0
    def calculate(self):
        self.temp_position = self.get_position()
        robotMove = almath.pose2DInverse(
            self.init_position) * self.temp_position
        if robotMove.theta > 0:
            x_update = self.coordinate_init[1] + robotMove.x
            y_update = self.coordinate_init[0] + robotMove.y
            theta_temp = self.temp_position.theta - self.init_position.theta
            self.coordinate_update = [y_update, x_update, theta_temp]
        else:
            x_update = self.coordinate_init[1] + robotMove.x
            y_update = self.coordinate_init[0] - robotMove.y
            theta_temp = self.temp_position.theta - self.init_position.theta
            self.coordinate_update = [y_update, x_update, theta_temp]

        return self.coordinate_update
Example #13
0
def main(robotIP, PORT=9559):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Pose Init
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = motionProxy.getRobotPosition(useSensorValues)
    print "Simple Robot Position", result

    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = True
    initRobotPosition = almath.Pose2D(
        motionProxy.getRobotPosition(useSensorValues))
    print "Robot Position", initRobotPosition
    # Make the robot move
    # motionProxy.moveTo(0.1, 0.0, 0.2)

    # 4 foot (4 Ft, 48 in)
    # x = 1.2192
    # y = 0
    # theta = 0
    # print x, y, theta
    # motionProxy.moveTo(x, y, theta)

    # 2 foot (2 ft, 24in)
    x = 0.6096
    y = 0
    theta = 0
    print "Move to commands:", x, y, theta
    motionProxy.moveTo(x, y, theta)

    endRobotPosition = almath.Pose2D(
        motionProxy.getRobotPosition(useSensorValues))

    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
    print "Robot Move:", robotMove
    print "End Robot Position:", motionProxy.getRobotPosition(useSensorValues)

    # Go to rest position
    motionProxy.rest()
def main(session):
    """
    This example uses the getNextRobotPosition method.
    """
    # Get the services ALMotion & ALRobotPosture.

    motion_service = session.service("ALMotion")
    posture_service = session.service("ALRobotPosture")

    # Wake up robot
    motion_service.wakeUp()

    # Send NAO to Pose Init
    posture_service.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    result = motion_service.getNextRobotPosition()
    print "Next Robot Position", result

    # Example showing how to use this information to know the robot's diplacement
    # during the move process.

    # Make the robot move
    motion_service.moveTo(0.6, 0.0, 0.5,
                          _async=True)  # No blocking due to post called
    time.sleep(1.0)
    initRobotPosition = almath.Pose2D(motion_service.getNextRobotPosition())

    # Make the robot move
    motion_service.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = almath.Pose2D(motion_service.getNextRobotPosition())

    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
    print "Robot Move :", robotMove

    # Go to rest position
    motion_service.rest()
Example #15
0
def walking(motionProxy, X, Y, Theta):
    # Enable arms control by move algorithm
    motionProxy.setMoveArmsEnabled(True, True)

    # FOOT CONTACT PROTECTION
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    # get robot position before move
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))

    motionProxy.post.moveTo(X, Y, Theta)
    # wait is useful because with post moveTo is not blocking function
    motionProxy.waitUntilMoveIsFinished()

    # get robot position after move
    endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))

    # compute and print the robot motion
    robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition

    # return an angle between [-PI, PI]
    robotMove.theta = almath.modulo2PI(robotMove.theta)
    print "Robot Move:", robotMove
Example #16
0
def walking(motionProxy, X, Y, Theta):
    # Enable arms control by move algorithm
    motionProxy.setMoveArmsEnabled(True, True)

    # FOOT CONTACT PROTECTION
    motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]])

    # get robot position before move
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))

    motionProxy.post.moveTo(X, Y, Theta)
    # wait is useful because with post moveTo is not blocking function
    motionProxy.waitUntilMoveIsFinished()

    # get robot position after move
    endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(False))

    # compute and print the robot motion
    robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition

    # return an angle between [-PI, PI]
    robotMove.theta = almath.modulo2PI(robotMove.theta)
    print "Robot Move:", robotMove
Example #17
0
def main(session):
    """
    This example uses the getRobotPosition method.
    """
    # Get the services ALMotion & ALRobotPosture.

    motion_service = session.service("ALMotion")
    posture_service = session.service("ALRobotPosture")

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Pose Init
    posture_service.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = motion_service.getRobotPosition(useSensorValues)
    print "Robot Position", result

    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = False
    initRobotPosition = almath.Pose2D(
        motion_service.getRobotPosition(useSensorValues))

    # Make the robot move
    motion_service.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = almath.Pose2D(
        motion_service.getRobotPosition(useSensorValues))

    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
    print "Robot Move:", robotMove

    # Go to rest position
    motion_service.rest()
Example #18
0
    # Second call of move API
    motionProxy.post.moveTo(0.5, 0.5, -0.0)

    # get the second foot steps vector
    footSteps2 = motionProxy.getFootSteps()

    # end experiment, begin compute

    # here we wait until the move process is over
    motionProxy.waitUntilMoveIsFinished()
    # then we get the final robot position
    robotPositionFinal = almath.Pose2D(motionProxy.getRobotPosition(False))

    # compute robot Move with the second call of move API
    # so between nextRobotPosition and robotPositionFinal
    robotMove = almath.pose2DInverse(nextRobotPosition) * robotPositionFinal
    print "Robot Move :", robotMove

    # end compute, begin plot

    if (HAS_PYLAB):
        #################
        # Plot the data #
        #################
        pyl.figure()
        printRobotPosition(robotPosition, 'black')
        printRobotPosition(nextRobotPosition, 'blue')
        printFootSteps(footSteps1, 'green', 'red')

        pyl.figure()
        printRobotPosition(robotPosition, 'black')
Example #19
0
    result = motionProxy.getNextRobotPosition()
    print "Next Robot Position", result

    # Example showing how to use this information to know the robot's diplacement
    # during the move process.

    # Make the robot move
    motionProxy.post.moveTo(0.6, 0.0, 0.5)  # No blocking due to post called
    time.sleep(1.0)
    initRobotPosition = m.Pose2D(motionProxy.getNextRobotPosition())

    # Make the robot move
    motionProxy.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = m.Pose2D(motionProxy.getNextRobotPosition())

    # Compute robot's' displacement
    robotMove = m.pose2DInverse(initRobotPosition) * endRobotPosition
    print "Robot Move :", robotMove


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_getnextrobotposition.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
def main(session):
    """
    Walk To: Small example to make Nao Walk follow a Dubins Curve.
    """
    # Get the services ALMotion & ALRobotPosture.

    motion_service = session.service("ALMotion")
    posture_service = session.service("ALRobotPosture")

    # Wake up robot
    motion_service.wakeUp()

    # Send robot to Stand Init
    posture_service.goToPosture("StandInit", 0.5)

    # first we defined the goal
    goal = almath.Pose2D(0.0, -0.4, 0.0)

    # We get the dubins solution (control points) by
    # calling an almath function

    circleRadius = 0.04
    # Warning : the circle use by dubins curve
    #           have to be 4*CircleRadius < norm(goal)
    dubinsSolutionAbsolute = almath.getDubinsSolutions(goal, circleRadius)

    # moveTo With control Points use relative commands but
    # getDubinsSolution return absolute position
    # So, we compute dubinsSolution in relative way
    dubinsSolutionRelative = []
    dubinsSolutionRelative.append(dubinsSolutionAbsolute[0])
    for i in range(len(dubinsSolutionAbsolute) - 1):
        dubinsSolutionRelative.append(dubinsSolutionAbsolute[i].inverse() *
                                      dubinsSolutionAbsolute[i + 1])

    # create a vector of moveTo with dubins Control Points
    moveToTargets = []
    for i in range(len(dubinsSolutionRelative)):
        moveToTargets.append([
            dubinsSolutionRelative[i].x, dubinsSolutionRelative[i].y,
            dubinsSolutionRelative[i].theta
        ])

    # Initialized the Move process and be sure the robot is ready to move
    # without this call, the first getRobotPosition() will not refer to the position
    # of the robot before the move process
    motion_service.moveInit()

    # get robot position before move
    robotPositionBeforeCommand = almath.Pose2D(
        motion_service.getRobotPosition(False))

    motion_service.moveTo(moveToTargets)

    # With MoveTo control Points, it's also possible to customize the gait parameters
    # motionProxy.moveTo(moveToTargets,
    #                    [["StepHeight", 0.001],
    #                     ["MaxStepX", 0.06],
    #                     ["MaxStepFrequency", 1.0]])

    # get robot position after move
    robotPositionAfterCommand = almath.Pose2D(
        motion_service.getRobotPosition(False))

    # compute and print the robot motion
    robotMoveCommand = almath.pose2DInverse(
        robotPositionBeforeCommand) * robotPositionAfterCommand
    print "The Robot Move Command: ", robotMoveCommand

    # Go to rest position
    motion_service.rest()
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = motionProxy.getRobotPosition(useSensorValues)
    print "Robot Position", result

    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = False
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))

    # Make the robot move
    motionProxy.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))

    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition)*endRobotPosition
    print "Robot Move:", robotMove


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_getrobotposition.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
Example #22
0
    # get robot position before move
    robotPositionBeforeCommand = m.Pose2D(motionProxy.getRobotPosition(False))

    motionProxy.moveTo(moveToTargets)

    # With MoveTo control Points, it's also possible to customize the gait parameters
    # motionProxy.moveTo(moveToTargets,
    #                    [["StepHeight", 0.001],
    #                     ["MaxStepX", 0.06],
    #                     ["MaxStepFrequency", 1.0]])

    # get robot position after move
    robotPositionAfterCommand = m.Pose2D(motionProxy.getRobotPosition(False))

    # compute and print the robot motion
    robotMoveCommand = m.pose2DInverse(
        robotPositionBeforeCommand) * robotPositionAfterCommand
    print "The Robot Move Command: ", robotMoveCommand


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python motion_moveToDubinsCurve.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
    postureProxy.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = motionProxy.getRobotPosition(useSensorValues)
    print "Robot Position", result
    """
    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = False
    initRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))

    # Make the robot move
    motionProxy.moveTo(0.1, 0.0, 0.2)

    endRobotPosition = almath.Pose2D(motionProxy.getRobotPosition(useSensorValues))
    """
    # Compute robot's' displacement
    robotMove = almath.pose2DInverse(initRobotPosition) * endRobotPosition
    print "Robot Move:", robotMove


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python almotion_getrobotposition.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
Example #24
0
            [dubinRel[i].x,
             dubinRel[i].y,
             dubinRel[i].theta] )

     motionNaoProxy.moveInit()

    # get robot position before move
    robotNaoPosnBefCommand  = m.Pose2D(motionNaoProxy.getRobotPosition(False))

    motionNaoProxy.moveTo( moveTarg )

    # get robot position after move
    robotNaoPosnAftCommand = m.Pose2D(motionNaoProxy.getRobotPosition(False))

    # compute and print the robot motion
    moveNaoComms = m.pose2DInverse(robotNaoPosnBefCommand)*robotNaoPosnAftCommand
    print "The Robot Move Command: ", moveNaoComms

    # Go to rest position
    motionNaoProxy.rest()

if __name__ == "__main__":
    parseFile = argparse.ArgumentParser()
    parseFile.add_argument("--ip", type=str, default="127.0.0.1",
                        help="Robot ip address")
    parseFile.add_argument("--port", type=int, default=52742,
                        help="Robot port number")

    args = parseFile.parse_args()
    main(args.ip, args.port)
Example #25
0
    # Second call of move API
    motionProxy.post.moveTo(0.3, 0.0, -0.5)

    # get the second foot steps vector
    footSteps2 = motionProxy.getFootSteps()

    # end experiment, begin compute

    # here we wait until the move process is over
    motionProxy.waitUntilMoveIsFinished()
    # then we get the final robot position
    robotPositionFinal = almath.Pose2D(motionProxy.getRobotPosition(False))

    # compute robot Move with the second call of move API
    # so between nextRobotPosition and robotPositionFinal
    robotMove = almath.pose2DInverse(nextRobotPosition)*robotPositionFinal
    print "Robot Move :", robotMove

    # end compute, begin plot

    if (HAS_PYLAB):
      #################
      # Plot the data #
      #################
      pyl.figure()
      printRobotPosition(robotPosition, 'black')
      printRobotPosition(nextRobotPosition, 'blue')
      printFootSteps(footSteps1, 'green', 'red')

      pyl.figure()
      printRobotPosition(robotPosition, 'black')
    motionProxy.moveInit()

    # get robot position before move
    robotPositionBeforeCommand  = m.Pose2D(motionProxy.getRobotPosition(False))

    motionProxy.moveTo( moveToTargets )

    # With MoveTo control Points, it's also possible to customize the gait parameters
    # motionProxy.moveTo(moveToTargets,
    #                    [["StepHeight", 0.001],
    #                     ["MaxStepX", 0.06],
    #                     ["MaxStepFrequency", 1.0]])

    # get robot position after move
    robotPositionAfterCommand = m.Pose2D(motionProxy.getRobotPosition(False))

    # compute and print the robot motion
    robotMoveCommand = m.pose2DInverse(robotPositionBeforeCommand)*robotPositionAfterCommand
    print "The Robot Move Command: ", robotMoveCommand


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python motion_moveToDubinsCurve.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
def main(robotIP, PORT=9559):

    motionProxy  = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    # Wake up robot
    motionProxy.wakeUp()

    # Send robot to Stand Init
    postureProxy.goToPosture("StandInit", 0.5)

    # first we defined the goal
    goal = m.Pose2D(0.0, -0.4, 0.0)

    # We get the dubins solution (control points) by
    # calling an almath function

    circleRadius = 0.04
    # Warning : the circle use by dubins curve
    #           have to be 4*CircleRadius < norm(goal)
    dubinsSolutionAbsolute = m.getDubinsSolutions(goal, circleRadius)

    # moveTo With control Points use relative commands but
    # getDubinsSolution return absolute position
    # So, we compute dubinsSolution in relative way
    dubinsSolutionRelative = []
    dubinsSolutionRelative.append(dubinsSolutionAbsolute[0])
    for i in range(len(dubinsSolutionAbsolute)-1):
        dubinsSolutionRelative.append(
                dubinsSolutionAbsolute[i].inverse() *
                dubinsSolutionAbsolute[i+1])

    # create a vector of moveTo with dubins Control Points
    moveToTargets = []
    for i in range(len(dubinsSolutionRelative)):
        moveToTargets.append(
            [dubinsSolutionRelative[i].x,
             dubinsSolutionRelative[i].y,
             dubinsSolutionRelative[i].theta] )

    # Initialized the Move process and be sure the robot is ready to move
    # without this call, the first getRobotPosition() will not refer to the position
    # of the robot before the move process
    motionProxy.moveInit()

    # get robot position before move
    robotPositionBeforeCommand  = m.Pose2D(motionProxy.getRobotPosition(False))

    motionProxy.moveTo( moveToTargets )

    # With MoveTo control Points, it's also possible to customize the gait parameters
    # motionProxy.moveTo(moveToTargets,
    #                    [["StepHeight", 0.001],
    #                     ["MaxStepX", 0.06],
    #                     ["MaxStepFrequency", 1.0]])

    # get robot position after move
    robotPositionAfterCommand = m.Pose2D(motionProxy.getRobotPosition(False))

    # compute and print the robot motion
    robotMoveCommand = m.pose2DInverse(robotPositionBeforeCommand)*robotPositionAfterCommand
    print "The Robot Move Command: ", robotMoveCommand

    # Go to rest position
    motionProxy.rest()
Example #28
0
    initRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    X = 0.3
    Y = 0.1
    Theta = math.pi/2.0
    motionProxy.post.moveTo(X, Y, Theta)
    # wait is useful because with post moveTo is not blocking function
    motionProxy.waitUntilMoveIsFinished()

    #####################
    ## get robot position after move
    #####################
    endRobotPosition = m.Pose2D(motionProxy.getRobotPosition(False))

    #####################
    ## compute and print the robot motion
    #####################
    robotMove = m.pose2DInverse(initRobotPosition)*endRobotPosition
    print "Robot Move :", robotMove


if __name__ == "__main__":
    robotIp = "127.0.0.1"

    if len(sys.argv) <= 1:
        print "Usage python motion_moveTo.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
    def walk(self):
        # x, y, theta = 0
        global audio, r
        audio = r = None
        r = sr.Recognizer()

        count = 0
        self.motion.wakeUp()

        names = "HeadPitch"
        angles = -30.0 * m.TO_RAD
        fractionMaxSpeed = 0.1
        self.motion.setAngles(names, angles, fractionMaxSpeed)
        time.sleep(0.1)
        # resolution
        # kQQVGA (160x120), kQVGA (320x240), kVGA (640x480) or
        # k4VGA (1280x960, only with the HD camera).
        # color space desired
        # kYuvColorSpace, kYUVColorSpace, kYUV422InterlacedColorSpace, kRGBColorSpace, etc.
        # frames per second
        # Finally, select the minimal number of frames per second (fps) that your
        # vision module requires up to 30fps.
        name_id = self.video_service.subscribe("walkToH", 2, 11, 20)
        print("ALTracker successfully started, now show your face to robot!")
        print("Use Ctrl+c to stop this script.")
        pixel = 640
        try:
            while True:
                real_focal_length = self.get_focal_length()
                if real_focal_length == "no face detected":
                    self.tts.say("no face detection please try again")
                    continue
                else:
                    self.tts.say("ok I got your face ")
                    print("face detected")
                    break
            self.motion.moveInit()
            robotPositionBeforeCommand = m.Pose2D(
                self.motion.getRobotPosition(False))

            while True:
                count = count + 1
                nao_images = self.video_service.getImageRemote(name_id)
                if nao_images is None:
                    print("Can't capture frames from Nao's camera")
                    break

                cv_images = self.to_cv_img(nao_images)
                # time.sleep(0.1)

                img = imutils.resize(cv_images,
                                     width=min(pixel, cv_images.shape[1]))

                marker = self.find_face(img, face_cascade)
                object_marker = self.find_object(img, obj_cascade)

                if len(object_marker) is not 1:
                    # object_marker = [[0, 0, 0, 0]]
                    print("-------")
                    print(marker)
                    x, y, theta = self.foot_step(marker, real_focal_length)
                else:
                    for (xA, yA, xB, yB) in object_marker:
                        ya_max = yA
                        yb_max = yB
                        half_length = (xB - xA) / 2
                        obj_pix_value = xA + half_length

                    pix_obj_height = yb_max - ya_max
                    if pix_obj_height == 0:
                        return 0, 0, 0
                    print("-------")
                    print(object_marker)
                    inches = self.distance_to_camera(KNOWN_PERSON_HEIGHT,
                                                     real_focal_length,
                                                     pix_obj_height)
                    print("object detected", "%.2f inches" % inches)
                    print("object detected", obj_pix_value)
                    # print("-------")

                    if inches > 40.0:
                        x, y, theta = self.foot_step(marker, real_focal_length)

                    elif 40 > inches > 0:
                        if (pixel / 2) - 80 <= obj_pix_value:
                            x = 0
                            y = 0.3
                            theta = 0

                        elif obj_pix_value < (pixel / 2) + 80:
                            x = 0
                            y = -0.3
                            theta = 0

                        else:
                            x, y, theta = self.foot_step(
                                marker, real_focal_length)
                    else:
                        x = 0
                        y = 0
                        theta = 0

                frequency = 0

                self.motion.post.moveTo(x, y, theta, frequency)
                self.motion.waitUntilMoveIsFinished()
                #self.motion.setWalkTargetVelocity(0.0, 0.0, 0.0, 0.0)
                robotPositionAfterCommand = m.Pose2D(
                    self.motion.getRobotPosition(False))

                # compute and print the robot motion
                robotMoveCommand = m.pose2DInverse(
                    robotPositionBeforeCommand) * robotPositionAfterCommand
                print("The Robot Move Command: ", robotMoveCommand)
                print("-------")

                if count > 20:
                    # print(1)
                    self.headPitchYaw()
                    print("I'm tired, can I have a rest?")
                    self.tts.say("I'm tired \\pau=500\\ can I have a rest ")
                    self.motion.rest()
                    # ---------> Recording <---------
                    self.record.stopMicrophonesRecording()
                    print('Start recording...')
                    # tts.say("start recording...")
                    record_path = speech_record_file_path

                    self.record.startMicrophonesRecording(
                        record_path, 'wav', 16000, (0, 0, 1, 0))
                    time.sleep(3)
                    self.record.stopMicrophonesRecording()
                    print('stop recording')
                    # tts.say("stop recording")

                    # ---------> Speech recognition <---------
                    with sr.WavFile("./src/speechRecord.wav") as source:
                        audio = r.record(source)
                    text = r.recognize_google(audio)
                    print(text)
                    if text == "no":
                        count = 0
                        self.motion.wakeUp()
                        continue
                    elif text == "yes":
                        self.tts.say(
                            "okay \\pau=500\\ I'll have a break\\pau=500\\ touch my front head if you want to play"
                        )
                        break
                    else:
                        self.tts.say(
                            "I did not get you\\pau=500\\ I'll have a break ")
                        break
                else:
                    continue

        except SystemExit as e:
            print("System error : {0}".format(e))
        except OSError as err:
            print("OS error: {0}".format(err))
        else:
            print("Unexpected error:", sys.exc_info()[0])
        finally:
            print("Unsubscribe the video!")
            self.video_service.unsubscribe(name_id)
            self.motion.rest()
Example #30
0
    try:
        motionNaoProxy = ALProxy("ALMotion", NaoIP, PORT)
    except Exception, e:
        print "Error was: ", e
        sys.exit(1)

    try:
        postNaoProxy = ALProxy("ALRobotPosture", NaoIP, PORT)
    except Exception, e:
        print "Error was: ", e

    postNaoProxy.goToPosture("StandInit", 0.5)
    useSensValues = False
    result = motionNaoProxy.getRobotPosition(useSensValues)
    print "Nao Position", result
    useSensValues = False
    initNaoPosition = almath.Pose2D(
        motionNaoProxy.getRobotPosition(useSensValues))

    motionNaoProxy.moveTo(41, 1, 0.0)

    endNaoPosition = almath.Pose2D(
        motionNaoProxy.getRobotPosition(useSensValues))

    NaoMove = almath.pose2DInverse(initRobotPosition) * endNaoPosition
    print "Nao Move:", NaoMove


if __name__ == "__main__":
    NaoIp = "127.0.0.1"
    main(NaoIp)