예제 #1
0
파일: BasicMotion.py 프로젝트: burst/burst
def killAllTasks():
    # Deprecated.
    motionProxy = burst.getMotionProxy()
    motionProxy.killAll()
    # motionProxy.setBalanceMode(1)
    # motionProxy.setSupportMode(0)
    return None
예제 #2
0
파일: BasicMotion.py 프로젝트: burst/burst
def setStiffness(stiffness):
    motionProxy = burst.getMotionProxy().post
    pid = motionProxy.setBodyStiffness(stiffness)
    # Dirty hack:
    if stiffness == 0.0:
        stopWalking()
    return pid
예제 #3
0
파일: BasicMotion.py 프로젝트: burst/burst
def slowStraightWalk(distance):
    motionProxy = burst.getMotionProxy().post
    motionProxy.setWalkArmsConfig(
        100.0 * burst.motion.TO_RAD, 10.0 * burst.motion.TO_RAD, 30.0 * burst.motion.TO_RAD, 10.0 * burst.motion.TO_RAD
    )
    motionProxy.setWalkArmsEnable(True)
    motionProxy.setWalkExtraConfig(4.5, -4.5, 0.22, 2.0)
    motionProxy.setWalkConfig(0.05, 0.04, 0.04, 0.4, 0.01, 0.00)
    motionProxy.addWalkStraight(distance, 80)
    return motionProxy.walk()
예제 #4
0
파일: BasicMotion.py 프로젝트: burst/burst
def initPosition():
    motionProxy = burst.getMotionProxy().post
    joints = [
        "RShoulderPitch",
        "RShoulderRoll",
        "RElbowYaw",
        "RElbowRoll",
        "LShoulderPitch",
        "LShoulderRoll",
        "LElbowYaw",
        "LElbowRoll",
        "LHipYawPitch",
        "RHipPitch",
        "RHipRoll",
        "LHipPitch",
        "LHipRoll",
        "RKneePitch",
        "LKneePitch",
        "RAnklePitch",
        "RAnkleRoll",
        "LAnklePitch",
        "LAnkleRoll",
    ]
    times = len(joints) * [[3]]
    angles = [
        [50],
        [0],
        [10],
        [35],
        [50],
        [0],
        [0],
        [-35],
        [-10],
        [-40],
        [0],
        [-40],
        [0],
        [125],
        [125],
        [-70],
        [0],
        [-70],
        [0],
    ]
    for i in range(0, len(angles)):
        for j in range(0, len(angles[i])):
            angles[i][j] *= burst.motion.TO_RAD
    return motionProxy.doMove(joints, angles, times, 1)
예제 #5
0
파일: BasicMotion.py 프로젝트: burst/burst
def fastStraightWalk(distance):
    motionProxy = burst.getMotionProxy().post
    motionProxy.setWalkExtraConfig(3.5, -3.5, 0.23, 3.0)
    motionProxy.setWalkConfig(0.04, 0.02, 0.02, 0.3, 0.015, 0.018)
    motionProxy.addWalkStraight(distance, 25)
    return motionProxy.walk()
예제 #6
0
파일: BasicMotion.py 프로젝트: burst/burst
def addWalkStraight(distance, samples):
    motionProxy = burst.getMotionProxy().post
    return motionProxy.addWalkStraight(distance, samples)
예제 #7
0
파일: BasicMotion.py 프로젝트: burst/burst
def moveHead(x, y):
    motionProxy = burst.getMotionProxy()
    headStiffnessOn()
    motionProxy.gotoChainAngles("Head", [float(x), float(y)], 1, 1)
예제 #8
0
파일: BasicMotion.py 프로젝트: burst/burst
def closeRightHand():
    motionProxy = burst.getMotionProxy().post
    return motionProxy.closeHand("RHand")
예제 #9
0
파일: BasicMotion.py 프로젝트: burst/burst
def addTurn(degrees):
    motionProxy = burst.getMotionProxy().post
    motionProxy.addTurn(float(degrees), 60)
예제 #10
0
파일: BasicMotion.py 프로젝트: burst/burst
def headStiffnessOff():
    motionProxy = burst.getMotionProxy().post
    motionProxy.setChainStiffness("Head", 0.0, 0)
예제 #11
0
파일: Shoot.py 프로젝트: alon/burst
# TODO: This entire file looks like shit.

import time
import burst

burst.init()

motion = burst.motion

def do():
	broker = burst.getBroker()
    motionProxy = burst.getMotionProxy()

	# Define The Initial Position
	kneeAngle = 60.0 * burst.motion.TO_RAD
	torsoAngle = 0.0 * burst.motion.TO_RAD
	wideAngle = -3.0 * burst.motion.TO_RAD

	#Get the Number of Joints
	NumJoints = len(motionProxy.getBodyJointNames())

	# Define The Initial Position
	if (NumJoints == 22) :
	  InitialPosition = [0.0 * motion.TO_RAD, 0.0 * motion.TO_RAD,
		 120.0 * motion.TO_RAD, 15.0 * motion.TO_RAD, -80.0 * motion.TO_RAD, -80.0 * motion.TO_RAD,
		 0.0, wideAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, -wideAngle,
		 0.0, -wideAngle, -kneeAngle/2-torsoAngle, kneeAngle, -kneeAngle/2, wideAngle,
		 120.0 * motion.TO_RAD, -15.0 * motion.TO_RAD, 80.0 * motion.TO_RAD, 80.0 * motion.TO_RAD]
	elif (NumJoints == 26) :
	  InitialPosition = [0.0 * motion.TO_RAD, 0.0 * motion.TO_RAD,
		 120.0 * motion.TO_RAD, 15.0 * motion.TO_RAD, -80.0 * motion.TO_RAD, -80.0 * motion.TO_RAD, 0.0 * motion.TO_RAD,0.0,
예제 #12
0
파일: BasicMotion.py 프로젝트: burst/burst
def unflexRightArm():
    motionProxy = burst.getMotionProxy().post
    x = motionProxy.getChainAngles("RArm")
    x[3] = -math.pi
    return motionProxy.post.gotoChainAngles("RArm", x, 3.0, 1)
예제 #13
0
파일: BasicMotion.py 프로젝트: burst/burst
def zeroPosition():
    motionProxy = burst.getMotionProxy().post
    return motionProxy.gotoBodyAnglesWithSpeed(26 * [0.0], 15, burst.motion.INTERPOLATION_SMOOTH)
예제 #14
0
파일: BasicMotion.py 프로젝트: burst/burst
def openRightHand():
    motionProxy = burst.getMotionProxy().post
    return motionProxy.openHand("RHand")
예제 #15
0
파일: BasicMotion.py 프로젝트: burst/burst
def stopWalking():
    motionProxy = burst.getMotionProxy().post
    motionProxy.clearFootsteps()
    return None
예제 #16
0
파일: BasicMotion.py 프로젝트: burst/burst
def openLeftHand():
    motionProxy = burst.getMotionProxy().post
    return motionProxy.openHand("LHand")
예제 #17
0
파일: BasicMotion.py 프로젝트: burst/burst
def closeLeftHand():
    motionProxy = burst.getMotionProxy().post
    return motionProxy.closeHand("LHand")
예제 #18
0
파일: GetUpFromBack.py 프로젝트: alon/burst
def do():
	motion = burst.motion
	ALMotion_Proxy = burst.getMotionProxy()

	TimeInterpolation = 0.05
	ALMotion_Proxy.setBodyStiffness(1.0,TimeInterpolation)
	time.sleep(TimeInterpolation)

	joints = ["HeadPitch","HeadYaw","LAnklePitch","LAnkleRoll","LElbowRoll","LElbowYaw","LHipPitch","LHipRoll","LHipYawPitch","LKneePitch","LShoulderPitch","LShoulderRoll","RAnklePitch","RAnkleRoll","RElbowRoll","RElbowYaw","RHipPitch","RHipRoll","RKneePitch","RShoulderPitch","RShoulderRoll"];
	angles = [[0,0],
	[3.37175e-007,3.37175e-007],
	[0.523599,0.523599],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[1.5708,1.5708],
	[0.523599,0.523599],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[0,0],
	[-1.5708,-1.5708],
	];

	times = [[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	[1.9,2.9],
	];


	try:
	  moveId = ALMotion_Proxy.doMove(joints, angles, times, 1);
	except Exception,e:
	  print "get up from back test Failed"
	  exit(1)
예제 #19
0
파일: BasicMotion.py 프로젝트: burst/burst
def turn(degrees):
    motionProxy = burst.getMotionProxy().post
    motionProxy.addTurn(float(degrees), 60)
    return motionProxy.walk()
예제 #20
0
파일: __init__.py 프로젝트: alon/burst
    def __init__(self):
        if burst.options.trace_proxies:
            print "INFO: Proxy tracing is on"
            # avoid logging None objects - like when getSpeechProxy returns None
            callWrapper = lambda name, obj: (obj and LogCalls(name, obj) or obj)
        else:
            callWrapper = lambda name, obj: obj

        self._memory = callWrapper("ALMemory", burst.getMemoryProxy(deferred=True))
        self._motion = callWrapper("ALMotion", burst.getMotionProxy(deferred=True))
        self._sentinel = callWrapper("ALSentinel", burst.getSentinelProxy(deferred=True))
        self._speech = callWrapper("ALSpeech", burst.getSpeechProxy(deferred=True))
        self._video = callWrapper("ALVideoDevice", burst.getALVideoDeviceProxy(deferred=True))
        self._leds = callWrapper("ALLeds", burst.getLedsProxy(deferred=True))
        self._imops = callWrapper("imops", burst.getImopsProxy(deferred=True))

        if burst.options.run_sonar:
            self._sonar = callWrapper("ALSonar", burst.getSonarProxy(deferred=True))
        self._events = set()
        self._deferreds = []

        self.burst_deferred_maker = BurstDeferredMaker()

        # This makes sure stuff actually works if nothing is being updated on the nao.
        self._default_proxied_variable_value = 0.0

        # We do memory.getListData once per self.update, in one go.
        # later when this is done with shared memory, it will be changed here.
        # initialize these before initializing objects that use them (Ball etc.)
        default_vars = self.getDefaultVars()
        self._vars_to_get_set = set()
        self._vars_to_get_list = list()
        self.vars = {} # no leading underscore - meant as a public interface (just don't write here, it will be overwritten every update)
        self.addMemoryVars(default_vars)
        self._shm = None

        self.time = 0.0
        self.start_time = time()     # construction time

        # Variables for body joint angles from dcm
        self._getAnglesMap = dict([(joint,
            'Device/SubDeviceList/%s/Position/Sensor/Value' % joint)
            for joint in self.jointnames])
        if self.connected_to_webots and is_120:
            print "World: Using ALMotion for body angles"
            self._updateBodyAngles = self._updateBodyAngles_from_ALMotion
        else:
            print "World: Using DCM for body angles"
            self._body_angles_vars = self._getAnglesMap.values()
            self._updateBodyAngles = self._updateBodyAngles_from_DCM
            self.addMemoryVars(self._body_angles_vars)
        self.body_angles = [0.0] * 26

        # Variables for Inclination angles
        self._inclination_vars = ['Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value',
            'Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value']
        self.addMemoryVars(self._inclination_vars)

        self._recorded_vars = self._getRecorderVariableNames()

        print "world will record (if asked) %s vars" % len(self._recorded_vars)
        self._recorded_header = self._recorded_vars
        self._record_basename = World.running_on_nao and '/media/userdata' or '/tmp'

        # Stuff that we prefer the users use directly doesn't get a leading
        # underscore
        #  * Vision recognized objects
        self.ball = Ball(self)
        # Goals Notes: We start at UNCONFIGURED State, we record all the variables
        # but only start using them once we configure all the goal according to our team and
        # goal color.
        our_lp_xy = field.our_goal.top_post.xy        # left is from pov of goalie looking at opponent goal.
        our_rp_xy = field.our_goal.bottom_post.xy
        opposing_lp_xy = field.opposing_goal.bottom_post.xy
        opposing_rp_xy = field.opposing_goal.top_post.xy
        self.our_goal = Goal(name='OurGoal', which_team=OUR_TEAM,
            world=self, left_name='OurLeftPost', right_name='OurRightPost',
            left_world=our_lp_xy, right_world=our_rp_xy)
        self.our_lp = self.our_goal.left
        self.our_rp = self.our_goal.right
        self.opposing_goal = Goal(name='OpposingGoal', which_team=OPPOSING_TEAM,
            world=self, left_name='OpposingLeftPost', right_name='OpposingRightPost',
            left_world=opposing_lp_xy, right_world=opposing_rp_xy)
        self.opposing_lp = self.opposing_goal.left
        self.opposing_rp = self.opposing_goal.right
        self.all_posts = set([self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp])
        self.all_posts_hack = [self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp]
        for name in VISION_POSTS_NAMES:
            self.addMemoryVars(GoalPost.getVarsForName(name))
        # TODO - other robots
        # Buttons, Leds (TODO: sonar,
        self.robot = Robot(self)
        # TODO - is computed used? should be renamed for legibility
        self.computed = Computed(self)
        # Self orientation and Location of self and other objects in field.
        # (passes some stuff into other objects)
        self.pose = Pose(self)
        self.localization = Localization(self)
        self.odometry = Odometry(self)

        # The Game-Status, Game-Controller and RobotData Trifecta # TODO: This is messy.
        self.playerSettings = PlayerSettings(self) # Start with the default settings. You will be configured later to the right ones by the referees.
        self.gameStatus = GameStatus(self, self.playerSettings)
        self._gameController = GameController(self.gameStatus)

        self._movecoordinator = MoveCoordinator(self)

        # All objects that we delegate the event computation and naoqi
        # interaction to.  TODO: we have the exact state of B-HUMAN, so we
        # could use exactly their solution, and hence this todo. We have
        # multiple objects that calculate their events based on ground truths
        # (naoqi proxies) and on calculated truths. We need to rerun them
        # every time something is updated, *in the correct order*. So right
        # now I'm hardcoding this by using an ordered list of lists, but
        # later this should be calculated by storing a "needed" and
        # "provided" list, just like B-HUMAN, and doing the sorting once (and
        # that value can be cached to avoid recomputing on each run).
        self._objects = [
            # All basic objects that rely on just naoproxies should be in the
            # first list
            [self._movecoordinator,
             self.ball, self.our_goal, self.opposing_goal,
             self.robot, self._gameController],
            [self.gameStatus],
            # anything that relies on basics but nothing else should go next
            [self],
            # self.computed should always be last
            [self.localization],
            [self.computed],
        ]

        # Another useful collection
        self.vision_objects = [self.ball, self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp]

        # logging variables
        self._logged_objects = [[obj, None] for obj in [self.ball]]
        self._object_to_filename = {self.ball: 'ball'}
        self._do_log_positions = burst.options.log_positions
        if self._do_log_positions:
            self._openPositionLogs()

        if burst.options.no_memory_updates:
            print "world: NO MEMORY UPDATES.. N-O   M-E-M-O-R-Y   U-P-D-A-T-E-S"
            self._updateMemoryVariables = self._updateMemoryVariables_noop
        else:
            # Try using shared memory to access variables
            if World.running_on_nao:
                #self.updateMmapVariablesFile() # // TODO -remove the file! it is EVIL
                SharedMemoryReader.tryToInitMMap()
                if SharedMemoryReader.isMMapAvailable():
                    print "world: using SharedMemoryReader"
                    if US_DISTANCES_VARNAME in self._vars_to_get_list:
                        self._vars_to_get_list.remove(US_DISTANCES_VARNAME)
                    self._shm = SharedMemoryReader(self._vars_to_get_list)
                    self._updateMemoryVariables = self._updateMemoryVariables_noop #(temp)
                    self._shm.openDeferred.addCallback(self._switchToSharedMemory).addErrback(log.err)
            if self._shm is None:
                print "world: using ALMemory"

        self.checkManModule()
예제 #21
0
파일: BasicMotion.py 프로젝트: burst/burst
def wait(pid):
    motionProxy = burst.getMotionProxy()
    motionProxy.wait(pid, 0)