Make the robot balance.
172153
def _balance(self):
        """"""
        while True and not self.stop_balance.is_set():

            # Reset the motors
            self.motor_left.reset()  # Reset the encoder
            self.motor_right.reset()
            self.motor_left.run_direct()  # Set to run direct mode
            self.motor_right.run_direct()

            # Initialize variables representing physical signals
            # (more info on these in the docs)

            # The angle of "the motor", measured in raw units,
            # degrees for the EV3).
            # We will take the average of both motor positions as
            # "the motor" angle, which is essentially how far the middle
            # of the robot has travelled.
            motor_angle_raw = 0

            # The angle of the motor, converted to RAD (2*pi RAD
            # equals 360 degrees).
            motor_angle = 0

            # The reference angle of the motor. The robot will attempt to
            # drive forward or backward, such that its measured position
            motor_angle_ref = 0
            # equals this reference (or close enough).

            # The error: the deviation of the measured motor angle from the
            # reference. The robot attempts to make this zero, by driving
            # toward the reference.
            motor_angle_error = 0

            # We add up all of the motor angle error in time. If this value
            # gets out of hand, we can use it to drive the robot back to
            # the reference position a bit quicker.
            motor_angle_error_acc = 0

            # The motor speed, estimated by how far the motor has turned in
            # a given amount of time.
            motor_angular_speed = 0

            # The reference speed during manouvers: how fast we would like
            # to drive, measured in RAD per second.
            motor_angular_speed_ref = 0

            # The error: the deviation of the motor speed from the
            # reference speed.
            motor_angular_speed_error = 0

            # The 'voltage' signal we send to the motor.
            # We calculate a new value each time, just right to keep the
            # robot upright.
            motor_duty_cycle = 0

            # The raw value from the gyro sensor in rate mode.
            gyro_rate_raw = 0

            # The angular rate of the robot (how fast it is falling forward
            # or backward), measured in RAD per second.
            gyro_rate = 0

            # The gyro doesn't measure the angle of the robot, but we can
            # estimate this angle by keeping track of the gyro_rate value
            # in time.
            gyro_est_angle = 0

            # Over time, the gyro rate value can drift. This causes the
            # sensor to think it is moving even when it is perfectly still.
            # We keep track of this offset.
            gyro_offset = 0

            # Start
            log.info("Hold robot upright. Press touch sensor to start.")
            self.sound.speak("Press touch sensor to start.")

            self.touch.wait_for_bump()

            # Read battery voltage
            voltage_idle = self.power_supply.measured_volts
            voltage_comp = self.power_voltage_nominal / voltage_idle

            # Offset to limit friction deadlock
            friction_offset = int(round(self.pwr_friction_offset_nom *
                                        voltage_comp))

            # Timing settings for the program
            # Time of each loop, measured in seconds.
            loop_time_target = self.timing_loop_msec / 1000
            loop_count = 0  # Loop counter, starting at 0

            # A deque (a fifo array) which we'll use to keep track of
            # previous motor positions, which we can use to calculate the
            # rate of change (speed)
            motor_angle_hist =\
                deque([0], self.motor_angle_history_length)

            # The rate at which we'll update the gyro offset (precise
            # definition given in docs)
            gyro_drift_comp_rate =\
                self.gyro_drift_compensation_factor *\
                loop_time_target * RAD_PER_SEC_PER_RAW_GYRO_UNIT

            # Calibrate Gyro
            log.info("-----------------------------------")
            log.info("Calibrating...")

            # As you hold the robot still, determine the average sensor
            # value of 100 samples
            gyro_calibrate_count = 100
            for i in range(gyro_calibrate_count):
                gyro_offset = gyro_offset + self._fast_read(self.gyro_file)
                time.sleep(0.01)
            gyro_offset = gyro_offset / gyro_calibrate_count

            # Print the result
            log.info("gyro_offset: " + str(gyro_offset))
            log.info("-----------------------------------")
            log.info("GO!")
            log.info("-----------------------------------")
            log.info("Press Touch Sensor to re-start.")
            log.info("-----------------------------------")
            self.sound.beep()

            # Remember start time
            prog_start_time = time.time()

            if self.debug:
                # Data logging
                data = OrderedDict()
                loop_times = OrderedDict()
                data['loop_times'] = loop_times
                gyro_readings = OrderedDict()
                data['gyro_readings'] = gyro_readings

            # Initial fast read touch sensor value
            touch_pressed = False

            # Driving and Steering
            speed, steering = (0, 0)

            # Record start time of loop
            loop_start_time = time.time()

            # Balancing Loop
            while not touch_pressed and not self.stop_balance.is_set():

                loop_count += 1

                # Check for drive instructions and set speed / steering
                try:
                    speed, steering = self.drive_queue.get_nowait()
                    self.drive_queue.task_done()
                except queue.Empty:
                    pass

                # Read the touch sensor (the kill switch)
                touch_pressed = self._fast_read(self.touch_file)

                # Read the Motor Position
                motor_angle_raw = ((self._fast_read(self.encoder_left_file) +
                                   self._fast_read(self.encoder_right_file)) /
                                   2.0)
                motor_angle = motor_angle_raw * RAD_PER_RAW_MOTOR_UNIT

                # Read the Gyro
                gyro_rate_raw = self._fast_read(self.gyro_file)

                # Busy wait for the loop to reach target time length
                loop_time = 0
                while(loop_time < loop_time_target):
                    loop_time = time.time() - loop_start_time
                    time.sleep(0.001)

                # Calculate most recent loop time
                loop_time = time.time() - loop_start_time

                # Set start time of next loop
                loop_start_time = time.time()

                if self.debug:
                    # Log gyro data and loop time
                    time_of_sample = time.time() - prog_start_time
                    gyro_readings[time_of_sample] = gyro_rate_raw
                    loop_times[time_of_sample] = loop_time * 1000.0

                # Calculate gyro rate
                gyro_rate = (gyro_rate_raw - gyro_offset) *\
                    RAD_PER_SEC_PER_RAW_GYRO_UNIT

                # Calculate Motor Parameters
                motor_angular_speed_ref =\
                    speed * RAD_PER_SEC_PER_PERCENT_SPEED
                motor_angle_ref = motor_angle_ref +\
                    motor_angular_speed_ref * loop_time_target
                motor_angle_error = motor_angle - motor_angle_ref

                # Compute Motor Speed
                motor_angular_speed =\
                    ((motor_angle - motor_angle_hist[0]) /
                     (self.motor_angle_history_length * loop_time_target))
                motor_angular_speed_error = motor_angular_speed
                motor_angle_hist.append(motor_angle)

                # Compute the motor duty cycle value
                motor_duty_cycle =\
                    (self.gain_gyro_angle * gyro_est_angle +
                     self.gain_gyro_rate * gyro_rate +
                     self.gain_motor_angle * motor_angle_error +
                     self.gain_motor_angular_speed *
                     motor_angular_speed_error +
                     self.gain_motor_angle_error_accumulated *
                     motor_angle_error_acc)

                # Apply the signal to the motor, and add steering
                self._set_duty(self.dc_right_file, motor_duty_cycle + steering,
                               friction_offset, voltage_comp)
                self._set_duty(self.dc_left_file, motor_duty_cycle - steering,
                               friction_offset, voltage_comp)

                # Update angle estimate and gyro offset estimate
                gyro_est_angle = gyro_est_angle + gyro_rate *\
                    loop_time_target
                gyro_offset = (1 - gyro_drift_comp_rate) *\
                    gyro_offset + gyro_drift_comp_rate * gyro_rate_raw

                # Update Accumulated Motor Error
                motor_angle_error_acc = motor_angle_error_acc +\
                    motor_angle_error * loop_time_target

            # Closing down & Cleaning up

            # Loop end time, for stats
            prog_end_time = time.time()

            # Turn off the motors
            self._fast_write(self.dc_left_file, 0)
            self._fast_write(self.dc_right_file, 0)

            # Wait for the Touch Sensor to be released
            while self.touch.is_pressed:
                time.sleep(0.01)

            # Calculate loop time
            avg_loop_time = (prog_end_time - prog_start_time) / loop_count
            log.info("Loop time:" + str(avg_loop_time * 1000) + "ms")

            # Print a stop message
            log.info("-----------------------------------")
            log.info("STOP")
            log.info("-----------------------------------")

            if self.debug:
                # Dump logged data to file
                with open("data.txt", 'w') as data_file:
                    json.dump(data, data_file)
303167
def _reset_internal(self):
        """"""
        super()._reset_internal()
        self.sim.data.qpos[self._ref_joint_pos_indexes] = self.mujoco_robot.init_qpos

        if self.has_gripper_right:
            self.sim.data.qpos[
                self._ref_joint_gripper_right_actuator_indexes
            ] = self.gripper_right.init_qpos

        if self.has_gripper_left:
            self.sim.data.qpos[
                self._ref_joint_gripper_left_actuator_indexes
            ] = self.gripper_left.init_qpos
44607
def dof(self):
        """"""
        dof = self.mujoco_robot.dof
        if self.has_gripper_left:
            dof += self.gripper_left.dof
        if self.has_gripper_right:
            dof += self.gripper_right.dof
        return dof
833118
def from_robot(cls, robot, **kwargs):
        """"""
        m = cls(len(robot.m_feats), len(robot.s_feats), **kwargs)
        return m
275656
def Reset(self):
        """"""
        self.pan = self.world_center
        self.desired_pan = self.pos
167218
def run(self):
        """"""

        t_last_click = -1

        while True:
            d = self.device.read(13)
            if d is not None and self._enabled:

                if d[0] == 1:  ## readings from 6-DoF sensor
                    self.y = convert(d[1], d[2])
                    self.x = convert(d[3], d[4])
                    self.z = convert(d[5], d[6]) * -1.0

                    self.roll = convert(d[7], d[8])
                    self.pitch = convert(d[9], d[10])
                    self.yaw = convert(d[11], d[12])

                    self._control = [
                        self.x,
                        self.y,
                        self.z,
                        self.roll,
                        self.pitch,
                        self.yaw,
                    ]

                elif d[0] == 3:  ## readings from the side buttons

                    # press left button
                    if d[1] == 1:
                        t_click = time.time()
                        elapsed_time = t_click - t_last_click
                        t_last_click = t_click
                        self.single_click_and_hold = True

                    # release left button
                    if d[1] == 0:
                        self.single_click_and_hold = False

                    # right button is for reset
                    if d[1] == 2:
                        self._reset_state = 1
                        self._enabled = False
                        self._reset_internal_state()
1360989
final void rebalance() {
        // Let's start distribute the vbuckets across the servers
        configurationRwLock.writeLock().lock();
        try {
            Info.incrementConfigRevision();
            List<MemcachedServer> nodes = activeServers();
            for (int ii = 0; ii < numVBuckets; ++ii) {
                Collections.shuffle(nodes);
                vbInfo[ii].setOwner(nodes.get(0));
                if (nodes.size() < 2) {
                    continue;
                }
                List<MemcachedServer> replicas = nodes.subList(1, nodes.size());
                if (replicas.size() > numReplicas) {
                    replicas = replicas.subList(0, numReplicas);
                }
                vbInfo[ii].setReplicas(replicas);
            }
        } finally {
            Info.incrementConfigRevision();
            configurationRwLock.writeLock().unlock();
        }
    }
1088599
def balance(self):
        """"""
        sin = Decimal("0.00")
        sout = Decimal("0.00")

        for t in self.trans:
            if t.amount < Decimal("0.00"):
                sout += t.amount
            else:
                sin += t.amount

        return sin, sout
644722
public void balance() {
        // System.out.print("Before root splitchar = "); System.out.println(sc[root]);

        int i = 0, n = length;
        String[] k = new String[n];
        char[] v = new char[n];
        Iterator iter = new Iterator();
        while (iter.hasMoreElements()) {
            v[i] = iter.getValue();
            k[i++] = (String)iter.nextElement();
        }
        init();
        insertBalanced(k, v, 0, n);

        // With uniform letter distribution sc[root] should be around 'm'
        // System.out.print("After root splitchar = "); System.out.println(sc[root]);
    }
752448
def set_robot_joint_positions(self, jpos):
        
        self.sim.data.qpos[self._ref_joint_pos_indexes] = jpos
        self.sim.forward()
903880
def ustep(self):
        """"""

        self.U += self.rsdl_r(self.AX, self.Y)
1084797
def move_forward(self, seconds=None):
        """"""
        self._move(speed=SPEED_MAX, steering=0, seconds=seconds)
124831
function (response) {
            // the overlap vector
            var overlap = response.overlapV;

            // FIXME: Respond proportionally to object mass

            // Move out of the other object shape
            this.ancestor.pos.sub(overlap);

            // adjust velocity
            if (overlap.x !== 0) {
                this.vel.x = ~~(0.5 + this.vel.x - overlap.x) || 0;
                if (this.bounce > 0) {
                    this.vel.x *= -this.bounce;
                }
            }
            if (overlap.y !== 0) {
                this.vel.y = ~~(0.5 + this.vel.y - overlap.y) || 0;
                if (this.bounce > 0) {
                    this.vel.y *= -this.bounce;
                }

                // cancel the falling an jumping flags if necessary
                var dir = Math.sign(this.gravity.y) || 1;
                this.falling = overlap.y >= dir;
                this.jumping = overlap.y <= -dir;
            }
        }
1173797
private void rebalance(Node<K, V> unbalanced, boolean insert) {
    for (Node<K, V> node = unbalanced; node != null; node = node.parent) {
      Node<K, V> left = node.left;
      Node<K, V> right = node.right;
      int leftHeight = left != null ? left.height : 0;
      int rightHeight = right != null ? right.height : 0;

      int delta = leftHeight - rightHeight;
      if (delta == -2) {
        Node<K, V> rightLeft = right.left;
        Node<K, V> rightRight = right.right;
        int rightRightHeight = rightRight != null ? rightRight.height : 0;
        int rightLeftHeight = rightLeft != null ? rightLeft.height : 0;

        int rightDelta = rightLeftHeight - rightRightHeight;
        if (rightDelta == -1 || (rightDelta == 0 && !insert)) {
          rotateLeft(node); // AVL right right
        } else {
          assert (rightDelta == 1);
          rotateRight(right); // AVL right left
          rotateLeft(node);
        }
        if (insert) {
          break; // no further rotations will be necessary
        }

      } else if (delta == 2) {
        Node<K, V> leftLeft = left.left;
        Node<K, V> leftRight = left.right;
        int leftRightHeight = leftRight != null ? leftRight.height : 0;
        int leftLeftHeight = leftLeft != null ? leftLeft.height : 0;

        int leftDelta = leftLeftHeight - leftRightHeight;
        if (leftDelta == 1 || (leftDelta == 0 && !insert)) {
          rotateRight(node); // AVL left left
        } else {
          assert (leftDelta == -1);
          rotateLeft(left); // AVL left right
          rotateRight(node);
        }
        if (insert) {
          break; // no further rotations will be necessary
        }

      } else if (delta == 0) {
        node.height = leftHeight + 1; // leftHeight == rightHeight
        if (insert) {
          break; // the insert caused balance, so rebalancing is done!
        }

      } else {
        assert (delta == -1 || delta == 1);
        node.height = Math.max(leftHeight, rightHeight) + 1;
        if (!insert) {
          break; // the height hasn't changed, so rebalancing is done!
        }
      }
    }
  }
1201720
def robots(self):
        """"""
        r = 'noindex' if self.is_noindex else 'index'
        r += ','
        r += 'nofollow' if self.is_nofollow else 'follow'
        return r
578615
void setBalance(
    int       b)
  {
    if ( (b == -1) || (b == 0) || (b == 1) )
      _balance = (short) b;
    else
     {
       String x = "Attempt to set invalid balance factor: " + b;
       throw new IllegalArgumentException(x);
     }
  }
1282134
function(force, pos){
            pos = pos||{x:0,y:0};
            this.body.applyForce(cp.v(force.x, force.y), cp.v(pos.x, pos.y));
        }