Python ALProxy.setCollisionProtectionEnabled方法代码示例

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setCollisionProtectionEnabled [as 别名]
    def __init__(self, robotIP, PORT):
        motionproxy = ALProxy("ALMotion", robotIP, PORT)
        postureproxy = ALProxy("ALRobotPosture", robotIP, PORT)
        # Wake up robot
        motionproxy.wakeUp()
        motionproxy.setCollisionProtectionEnabled("Arms", True)

        self.device = motionproxy
        self.postureProxy = postureproxy

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setCollisionProtectionEnabled [as 别名]
import math
import random
from naoqi import ALProxy

IP = ['192.168.3.6','192.168.3.9','192.168.3.8']


angles = []

port = 9559

for ip in IP:
	motion_proxy = ALProxy('ALMotion',ip,port)


	motion_proxy.setCollisionProtectionEnabled('Arms', True)

	part = 'Body'

	#time.sleep(1.0)

	body_names = motion_proxy.getBodyNames(part)
	body_limits = [motion_proxy.getLimits(l)[0] for l in body_names]
	body_limits_angles = [ [l[0],l[1]] for l in body_limits]


	motion_proxy.setStiffnesses(part,1.0)

	for i in range(100):

		target_angles = [ (angles[1]-angles[0])*random.random()+angles[0] for angles in body_limits_angles]

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setCollisionProtectionEnabled [as 别名]
def main(robotIP, pChainName, PORT=9559):
    ''' Example showing the effect of collision detection
        Nao bumps his chest with his left arm with collision detection enabled
        or disabled.
    '''

    ##################
    # Initialization #
    ##################

    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)

    # Get the robot configuration.
    robotConfig = motionProxy.getRobotConfig()
    robotName = ""
    for i in range(len(robotConfig[0])):
        if (robotConfig[0][i] == "Model Type"):
            robotName = robotConfig[1][i]

    ###############################
    # Arm motion bumping on torso #
    ###############################

    # Disable collision detection on chainName.
    pEnable = False
    success = motionProxy.setCollisionProtectionEnabled(pChainName, pEnable)
    if (not success):
        print("Failed to disable collision protection")
    time.sleep(1.0)

    # Make NAO's arm move so that it bumps its torso.
    pTargetName = "Torso"
    moveArm(motionProxy, pTargetName, robotName, pChainName)
    time.sleep(1.0)

    # Go back to pose init.
    postureProxy.goToPosture("StandInit", 1.0)

    # Enable collision detection on chainName.
    pEnable = True
    success = motionProxy.setCollisionProtectionEnabled(pChainName, pEnable)
    if (not success):
        print("Failed to enable collision protection")
    time.sleep(1.0)

    # Make NAO's arm move and see that it does not bump on the torso.
    pTargetName = "Torso"
    moveArm(motionProxy, pTargetName, robotName, pChainName)

    ##############################
    # Arm motion bumping on head #
    ##############################

    time.sleep(1.0)
    # Go back to pose init.
    postureProxy.goToPosture("StandInit", 1.0)
    # Disable collision detection on chainName.
    pEnable = False
    success = motionProxy.setCollisionProtectionEnabled(pChainName, pEnable)
    if (not success):
        print("Failed to disable collision protection")
    time.sleep(1.0)
    # Make NAO's arm move so that it bumps its head.
    pTargetName = "Head"
    moveArm(motionProxy, pTargetName, robotName, pChainName)

    time.sleep(1.0)
    # Go back to pose init.
    postureProxy.goToPosture("StandInit", 1.0)
    # Enable collision detection on chainName.
    pEnable = True
    success = motionProxy.setCollisionProtectionEnabled(pChainName, pEnable)
    if (not success):
        print("Failed to enable collision protection")
    # Make NAO's arm move and see that it does not bump on the head.
    pTargetName = "Head"
    moveArm(motionProxy, pTargetName, robotName, pChainName)

    time.sleep(1.0)
    # Go back to pose init.
    postureProxy.goToPosture("StandInit", 1.0)

    # Go to rest position
    motionProxy.rest()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setCollisionProtectionEnabled [as 别名]
import socket
import string  
from naoqi import ALProxy
import argparse
import almath
import motion
import time
import thread

robot_ip = '192.168.1.100'
robot_port = 9559
motionProxy=ALProxy("ALMotion",robot_ip,robot_port)
motionProxy.wakeUp()
enable = True
chainName = "Arms"
isSuccess = motionProxy.setCollisionProtectionEnabled(chainName, enable)
time.sleep(1)


def setUpAngles(RShoulderPitch,RShoulderRoll,RElbowYaw,RElbowRoll,LShoulderPitch,LShoulderRoll,LElbowYaw,LElbowRoll,RHipYawPitch,RHipRoll,RHipPitch,RKneePitch,LHipYawPitch,LHipRoll,LHipPitch,LKneePitch,period):
	names = ["RShoulderPitch","RShoulderRoll","RElbowYaw","RElbowRoll","LShoulderPitch","LShoulderRoll","LElbowYaw","LElbowRoll"]
	angles =[RShoulderPitch*almath.TO_RAD,RShoulderRoll*almath.TO_RAD,RElbowYaw*almath.TO_RAD,RElbowRoll*almath.TO_RAD,LShoulderPitch*almath.TO_RAD,LShoulderRoll*almath.TO_RAD,LElbowYaw*almath.TO_RAD,LElbowRoll*almath.TO_RAD]
	fractionMaxSpeed  = 0.3
	motionProxy.setAngles(names, angles, fractionMaxSpeed)
	time.sleep(0.5*period)
	print "---------------- Human Motion Imitation -----------------"

#Receive Thread
def receive(f,fp,s,ss):
	while 1: 
		try:

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setCollisionProtectionEnabled [as 别名]

#.........这里部分代码省略.........
			minPosLeftx = -MAXPOSX
			maxPosRightx = MAXPOSX
			minPosRightx = -MAXPOSX
			
		if handSide == constante.RIGHT_HAND:
			#ElbowRoll
			self.setAngleRight(angles, ELBOWROLL, 0.0, 1.5, 75, -40, palmPosz)
			
			#ShoulderRoll
			if palmPosx > maxPosRightx:
				if angles[0] <= -1.5:
					angles[0] = -1.5
				angles[0] -= 0.1
			elif palmPosx < minPosRightx:
				if angles[0] >= 0.0:
					angles[0] = 0.0
				angles[0] += 0.1
			
		else:
			#handSide = left
			
			#ShoulderRoll
			if palmPosx > maxPosLeftx:
				if angles[0] >= 1.5:
					angles[0] = 1.5
				angles[0] -= 0.1
			elif palmPosx < minPosLeftx:
				if angles[0] <= 0.0:
					angles[0] = 0.0
				angles[0] += 0.1
				
			#ElbowRoll
			self.setAngleLeft(angles, ELBOWROLL, -1.5, 0.0, 75, -40, palmPosz)
			
		#ShoulderPitch	
		self.setAngleLeft(angles, SHOULDERPITCH, -1.5, 1.5, 350, 150, palmPosy)
		
		if 0.78 > normal > -0.78:
			angles[3] = -normal
			
		for i in range(len(angles)):
			speed = abs(angles[i]-current[i])/2.0
			if speed < 0.05:
				speed = 0.05
			if speed > 0.5:
				speed = 0.5
				
			name = self.rightArmNames[i]
			if handSide == constante.LEFT_HAND:
				name = self.leftArmNames[i]
				
			# Make the movement
			self.motionProxy.setAngles(name, angles[i], speed)
	
	def setAngleRight(self, angles, indice, valueMin, valueMax, posMax, posMin, palmPos):
		if palmPos > posMax:
			if angles[indice] >= valueMax:
				angles[indice] = valueMax
			angles[indice]+= 0.1
		elif palmPos < posMin:
			if angles[indice] <= valueMin:
				angles[indice] = valueMin
			angles[indice] -= 0.1
		
	def setAngleLeft(self, angles, indice, valueMin, valueMax, posMax, posMin, palmPos):
		if palmPos > posMax:
			if angles[indice] <= valueMin:
				angles[indice] = valueMin
			angles[indice] -= 0.1
		elif palmPos < posMin:
			if angles[indice] >= valueMax:
				angles[indice] = valueMax
			angles[indice] += 0.1
						
	def setCollisionEnabled(self, chainName):
		#activate "Arms" anticollision
		enable    = True
		isSuccess  = self.motionProxy.setCollisionProtectionEnabled(chainName, enable)
		return isSuccess	
				
	def turnMotorOn(self):
		#Turn on the nao's motors
		names  = 'Body'
		stiffnessLists  = 1.0
		timeLists  = 1.0
		self.motionProxy.stiffnessInterpolation(names, stiffnessLists, timeLists)
		self.motorOn = True
		
	def testMotorOn(self):
		if not self.motorOn:
			self.turnMotorOn()
			
	def stiffnessOff(self):
		names  = 'Body'
		stiffnessLists  = 0.0
		timeLists  = 1.0
		self.motionProxy.stiffnessInterpolation(names, stiffnessLists, timeLists)
		self.motorOn = False
		
		self.behaviourProxy.stopAllBehaviors()

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setCollisionProtectionEnabled [as 别名]
class MotionController:
    # NAO Default head position in radians
    DEFAULT_HEAD_PITCH = -0.05
    DEFAULT_HEAD_YAW = 0.0

    """
    Represents a virtual controller for NAO's motion system
    """
    def __init__(self, robot_ip=None, robot_port=None):
        """
        :param robot_ip: The IP address of the robot
        :type robot_ip: str
        :param robot_port: The port of the robot
        :type robot_port: int
        Creates a new Virtual Controller for NAO
        """
        if robot_ip is None:
            robot_ip = nao.IP
        if robot_port is None:
            robot_port = nao.PORT

        self.ip = robot_ip
        self.port = robot_port

        # Connect and wake up the robot
        self.motion_proxy = ALProxy("ALMotion", robot_ip, robot_port)
        # self.motion_proxy.wakeUp()
        self.motion_proxy.setCollisionProtectionEnabled("Arms", True)
        self.motion_proxy.setMoveArmsEnabled(False, False)
        # self.moveHead(0.114, 0, radians=True)

    def getCameraTopPositionFromTorso(self):
        """
        :return: The 6D coordinates of the top camera of NAO
                 6D coordinates : (x-translat., y-translat., z-translat., x-rot, y-rot, z-rot)
        """
        return self.motion_proxy.getPosition("CameraTop",
                                             FRAME_TORSO,
                                             True)

    def getCameraBottomPositionFromTorso(self):
        """
        :return: The 6D coordinates of the bottom camera of NAO
                 6D coordinates : (x-translat., y-translat., z-translat., x-rot, y-rot, z-rot)
        """
        return self.motion_proxy.getPosition("CameraBottom",
                                             FRAME_TORSO,
                                             True)

    def getLeftHandPosition(self):
        """
        :return: The 6D coordinates of the left hand of NAO
                 6D coordinates : (x-translat., y-translat., z-translat., x-rot, y-rot, z-rot)
        """
        # print self.motion_proxy.getBodyNames("Chains")
        return self.motion_proxy.getPosition("LHand", FRAME_TORSO, True)

    def setLeftHandPosition(self, coord, mask=7, time_limit=3.0):
        """
        :param coord: the coordinates, relative to the robot torso, where the hand will be placed if possible.
                      6D coordinates : (x-translat., y-translat., z-translat., x-rot, y-rot, z-rot)
        :type coord: list
        :param mask: 6 bits unsigned integer that represents which axis will be moved / rotated.
                     The sum of the following values will set the mask.
                     1 : x-axis translation
                     2 : y-axis translation
                     4 : z-axis translation
                     8 : x-axis rotation
                     16: y-axis rotation
                     32: z-axis rotation
        :type mask: int
        :param time_limit: The maximum time, in seconds, that the move can take. The shorter the faster.
        :type time_limit: float
        Move the hand to the given coordinates if possible.
        """
        self.stand()
        self.motion_proxy.positionInterpolations("LArm", FRAME_TORSO, [tuple(coord)], mask, [time_limit])

    def moveAt(self, x, y, z_rot):
        """
        :param x: the distance to travel forward (negative value = backward) in meters
        :type x: float
        :param y: the distance to travel to the left (negative value = to the right) in meters
        :type y: float
        :param z_rot: the rotation angle, in radians, around the vertical axis
        :type z_rot: float
        Move the robot to a certain position, defined by the three parameters.
        """
        self.stand()
        self.motion_proxy.wakeUp()
        self.motion_proxy.moveTo(x, y, z_rot)

    def getLeftArmAngles(self):
        """
        :return: the angles in radians of the left arm of NAO in this order :
            [ShoulderPitch, ShoulderRoll, ElbowYaw, ElbowRoll, WristYaw]
        :rtype: list
        """
        joint_names = ["LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw"]
        use_sensors = True
#.........这里部分代码省略.........

# 需要导入模块: from naoqi import ALProxy [as 别名]
# 或者: from naoqi.ALProxy import setCollisionProtectionEnabled [as 别名]
class RobotController:
	def __init__(self, ip, port, simulation_mode=True):
		self.ip = ip
		self.port = port
		self.camProxy = ALProxy("ALVideoDevice", self.ip, self.port)
		self.postureProxy = ALProxy("ALRobotPosture", self.ip, self.port)
		self.motionProxy = ALProxy("ALMotion", self.ip, self.port)
		self.memoryProxy = ALProxy("ALMemory", self.ip, self.port)
		self.blobProxy = ALProxy("ALColorBlobDetection", self.ip, self.port)
		self.trackerProxy = ALProxy("ALTracker", self.ip, self.port)
		self.ttsProxy = ALProxy("ALTextToSpeech", self.ip, self.port)
		self.broker = ALBroker("pythonMotionBroker", "0.0.0.0", 0, 
			self.ip, self.port)

		if simulation_mode == False:
			self.autolifeProxy = ALProxy("ALAutonomousLife", self.ip, self.port)
			self.autolifeProxy.setState("disabled")

		self.topCamera = self.camProxy.subscribeCamera("topCamera_pycli", 
			0, vision_definitions.kVGA, vision_definitions.kBGRColorSpace,
			framerate)
		self.bottomCamera = self.camProxy.subscribeCamera(
			"bottomCamera_pycli", 1, vision_definitions.kQQVGA, 
			vision_definitions.kBGRColorSpace, framerate)

		self.motionProxy.wakeUp()
		self.enableExternalCollisionProtection()
		self.postureProxy.goToPosture("StandInit", 1.0)

	def stiffnessOn(self):
		# We use the "Body" name to signify the collection of all joints
		pNames = "Body"
		pStiffnessLists = 1.0
		pTimeLists = 1.0
		self.motionProxy.stiffnessInterpolation(pNames, pStiffnessLists, 
			pTimeLists)

	def stiffnessOff(self):
		# We use the "Body" name to signify the collection of all joints
		pNames = "Body"
		pStiffnessLists = 0.0
		pTimeLists = 1.0
		self.motionProxy.stiffnessInterpolation(pNames, pStiffnessLists, 
			pTimeLists)

	def enableArmCollisionProtection(self):
		chainName = "Arms"
		enable  = True
		isSuccess = self.motionProxy.setCollisionProtectionEnabled(chainName, 
			enable)
		pprint("Anticollision activation on arms: " + str(isSuccess))

	def disableArmCollisionProtection(self):
		chainName = "Arms"
		enable  = False
		isSuccess = self.motionProxy.setCollisionProtectionEnabled(chainName, 
			enable)
		pprint("Anticollision deactivation on arms: " + str(isSuccess))

	def enableExternalCollisionProtection(self):
		self.motionProxy.setExternalCollisionProtectionEnabled("All", True)

	def disableExternalCollisionProtection(self):
		self.motionProxy.setExternalCollisionProtectionEnabled("All", False)

	def headRotateAbsolute(self, degrees):
		# Example showing a single target angle for one joint
		# Interpolate the head yaw to 15 degrees in 1.0 second
		names      = "HeadYaw"
		angleLists = degrees*almath.TO_RAD
		timeLists  = 1.0 if (degrees == 0.0) else abs(15.0/degrees)
		isAbsolute = True
		self.motionProxy.angleInterpolation(names, angleLists, timeLists, 
			isAbsolute)

	def headPitchAbsolute(self, degrees):
		# Example showing a single target angle for one joint
		# Interpolate the head pitch to 15 degrees in 1.0 second
		names      = "HeadPitch"
		angleLists = degrees*almath.TO_RAD
		timeLists  = 1.0 if (degrees == 0.0) else abs(15.0/degrees)
		isAbsolute = True
		self.motionProxy.angleInterpolation(names, angleLists, timeLists, 
			isAbsolute)	

	def bodyRotateAbsolute(self, degrees):
		self.motionProxy.moveTo(0.0, 0.0, degrees*almath.TO_RAD)

	def bodyWalkForward(self, distance):
		self.motionProxy.moveTo(distance, 0.0, 0.0)

	def takePicture(self, camera):
		naoImage = self.camProxy.getImageRemote(camera)

		# Get the image size and pixel array.
		imageWidth = naoImage[0]
		imageHeight = naoImage[1]
		timestampUS = naoImage[5]
		array = naoImage[6]
		cameraID = naoImage[7]
#.........这里部分代码省略.........