Introduction

In this project, we built both the hardware and software for a social robot inspired by Pixar Animation Studio’s Luxo Jr.. For the base robot arm, we used the open-source mini 6-DOF manipulator platform that we developed (for our Master of Engineering project). The end-effector, which contains both the lamp and a Raspberry Pi camera, was custom designed and manufactured for this project. To achieve organic behavior, we implemented face-detection and tracking algorithms on the Raspberry Pi 3B+. In addition, we implemented teach and replay algorithms to train the robot arm for movement routines, in order to quickly and easily achieve sophisticated motion. Overall we were successful and created a robot that is lively and interacts with humans in its surroundings, powered by a resource-restricted embedded device.

Objectives

  • Achieve social robot design through implementation of humanistic behaviors
  • Design and manufacture the lamp end effector for the 6-DOF robot arm
  • Implement a high frame rate and accurate face detection algorithms for tracking
  • Program robot to organically interact with surroundings and a physical light switch

Design

Hardware

Mechnical
Fig.1 - CAD Design
Fig.2 - End-Effector Detail

All of the robot hardware was custom designed. The arm, as mentioned, was already completely designed and manufactured from a different project, and was almost entirely 3D printed. It uses LewanSoul LX-16A serial servos, which allow for much more sophisticated controls and feedback than conventional ones. Servo controls will be discussed in a later section. The arm has 6 degrees of freedom, meaning it has 6 independent joints, allowing the end-effector to achieve a great range of poses in 3D space. The design was done entirely in Autodesk Inventor, and all CAD files are released open-source in the Github repository.

The end-effector, which in this case is the lamp “head”, is newly designed and created for this project. It was also done in Autodesk Inventor and was modeled from a conventional desk lamp. It comes apart in three separate pieces. The largest piece is the lampshade, which also serves as the connection to the robot arm. An intermediary frame mounts the HDMI camera conversion circuit board, as well as the bulb. The last piece is the bulb, which has mounting and a cutout for a Raspberry Pi camera. The bulb is printed out of a special light conductive filament, allowing it to scatter the light from an LED strip mounted internally and function as a real bulb.

To allow for secure mounting and solid electrical connections, we also designed a custom plate attached to the robot arm base for fastening down all the different components. For the base robot arm, we mounted a desk clamp so that the base is firmly planted to react against the forces that the lamp “head” will generate in motion. To make the switch more easily actuated by the robot, we designed and mounted an extension to the flip lever.

Electrical
Fig.3 - Electrical Detail

There are a few different electrical systems on the robot. First is the Raspberry Pi, which is powered via the standard DC power supply. Then the servos take power and control signals through a proprietary adapter board, which buffers and controls both the TX/RX serial data flow into one wire and provides all of the main power. The servos run on anything between 6 and 8.4 volts, so we used a standard lab DC supply and chose 6.6 volts to be safe. The servo adapter board connects to the Pi via a USB cable, and signals and power of the servos are relayed to each other via daisy-chaining, from a 3-pin Molex Mini-SPOX wire. More details can be found on the manufacturer’s website.

For the lamp end-effector, there are two separate systems. First is the camera, which receives its power and transmits data through an HDMI adapter board. The adapter board is used instead of the typical ribbon cable is because the ribbon cable is susceptible to wear and tear easily, especially if the camera is constantly moving on the robot arm. The adapter board connects to the camera via a very short ribbon cable, funnels the data to an HDMI connector that has 16 pins (ribbon cable has 15), and is adapted on the other side back into a ribbon cable to plug into the Pi. This is a component that we purchased, and we also chose a spiral HDMI cable that is more flexible.

The second system is the LED inside the bulb. For that, we simply cut a three LED segment from a strip and stuck it inside the bulb. The segment takes anywhere between 7 and 12 volts to turn on, so we connected to a switch and a 9-volt battery. The battery is used so that the arm does not need another power supply, and the switch is used so that the robot can interactively turn on its light, under its own power. To detect the state of the light bulb, a simple voltage divider was used to reduce the input level and is fed into a GPIO pin. A full electrical diagram can be found below:

Fig.4 - Electrical Diagram

Software

Servo Control

The low-level servo control is achieved through the manufacturer provided python serial protocol, although we have customized it as well to achieve better performance. The python package pyserial was used for this. There were a few different functions that we utilized. First is send command, and its basic structure is a 2-byte new instruction signal (two of 0x55), followed by the instruction type which is 1 byte, then followed by the actual data for the specific instruction. This is used for all commands that the Pi sends to the servo, and the instructions can vary in length depending on data and type. The instruction types that we used include servo/motor mode setting, servo mode angle command, motor mode speed command, and unload servo.

The serial servos are more advanced compared to regular servos in that first, they communicate through serial, and second, they have servo and motor modes. They can also be daisy-chained, meaning running a single connection between servo pairs, and are individually addressable through unique IDs. Servo mode is the same as regular servos, where the master issues an angular command and the servo goes to that angle. In this mode, the serial servos are limited to 240 degrees, which is represented by a feedback value of 0 to 1000, effectively resulting in a precision of 0.24 degrees. The angular control is done by an internal control loop and resists external forces fairly well. The cool thing about these servos is that in addition to being able to go to a certain angle from this command, it also has a time setting as the second argument. The time argument varies between 0 and 30,000 milliseconds, telling the servo how long it has to go to the desired angle. A value of 0 simply means go to the angle as quickly as possible, which is what we used for our implementation. Motor mode on these servos turns them into DC motors, and the control input of power can be adjusted from -1000 to 1000. A negative value means turn counterclockwise, and vice versa. In this mode, the servo has no angular restrictions. Lastly, unload servo means to allow the servo to be back-driven with as little resistance as possible, which is useful when manually adjusting the arm.

There are also two read commands that are used to obtain data from the servos, read angle and read the temperature. Both work the same, by first sending the desired servo ID, along with the command instruction type. Then, a small delay is required (presumably for the buffer to switch states into receive), after which data comes back and is decoded. We have experimented with this and shortened the delay as much as possible, in order to increase the read and command frequencies. For the read angle command, as explained the value returned is between 0 and 1000, representing the number of 0.24 degrees measurement intervals. For the temperature command, it simply returns the temperature of the motor inside the servo. It is used to monitor the servo conditions under longer-term operation, as the arm can draw as much as 5 amps for demanding maneuvers

High-Level Commands
Fig.5 - RViz Model
Fig.6 - Solidworks Model
Fig.7 - Moveit! Path Planning

Originally, we planned to use ROS and Moveit! package for high-level controls and path planning. Thus, we built a full 3D package using Solidworks and ROS tools on an Ubuntu machine and verified its functionality using RViz. However, we found that due to the Raspberry Pi’s relatively low computational power, ROS doesn’t work very well on it, and RViz is not even supported. So we have resorted to record and play for the arms control while doing a simple proportional controller for two select joints.

Joint States Record and Replay

To simplify path generation, we developed a record-and-replay Python script. In essence, when we run the script, the robot would be manipulated manually to achieve the desired sequence of motion. For each timestamp, the corresponding states of every servo and the time value would be recorded into a row of a CSV file. Then, when we run the replay script, the robot would read the specific CSV file line by line and check the time. If the time duration is matched, each servo is assigned the recorded angular value and the movement is thus recreated. Otherwise, if the time duration is not yet matched, the loop busy waits until the time does match. Below shows an example of a CSV file.

With this mechanism fully functional, we were able to generate 9 different CSV file routines, including 2 for physically turning on the switch, and 7 for randomized dance routines of various lengths. We found that 5, 10, and 15 seconds were good lengths for routines. Although we could have generated more routines, we found that it actually worked well with this implementation, and it was difficult to tell repeats of routines due to the non-deterministic face tracking mixed in between.

Fig.8 - Routine Example (CSV File)
Face Detection

We tested different face detection methods running on the RPi. Considering the limited computation resources, we first adapted the traditional Haar-Cascade method by using OpenCV to solve our problem, and this detector lacked accuracy when the camera is only able to capture the side of faces. But it executes fast with 20-32 frames per second at (w=320, h=240) resolution. Then we tried to improve the detector by employing deep learning-based neural networks. We used MTCNN which is a capstone architecture in the face detection field and the accuracy is excellent. It was able to detect faces accurately even with a small area captured by the camera, but it runs a lot slower with approximately 1-2 frames per second, which cannot satisfy our minimum requirement. Lastly, we tried to use lightweight face detection models based on deep learning in order to get a balance between detection speed and accuracy. However, even the most portable model did not satisfy our requirement, with the MobileNet-SSD v2 running at about 3-4 frames per second. So we ended up using the Haar-Cascade feature extractor and detector for our final implementation.

Haar-Cascade is a machine learning-based approach where a cascade function is trained from a lot of positive and negative images. We downloaded the pre-trained model parameters and applied them to our script directly. However, to achieve more precise face detection, we improve the Haar-Cascade algorithm by following tricks: first, we set the minimum size for the potential bounding boxes to be 60x60 pixels, in order to avoid mismatch for small face-like objects. Further, we picked the bounding box with the largest area to refine the detection results. Lastly, we drew the refined facial bounding box onto the real-time frame by using OpenCV to visualize the performance of the algorithm. Even though this algorithm is not accurate enough for detecting side-way faces, we found that its precision of detecting frontal-face is accurate enough for facial tracking and its speed is far beyond our minimum requirement.

To achieve face tracking which is stated in detail in the below section, we had to calculate the distance between the center of the detected bounding box and the captured frame. We simply got the distance by first calculating the center coordinates of both the bounding box and the image frame, and then doing subtraction. Since we had to instruct the robot arm in which direction it should move next, we also got the direction information by the symbol of the subtraction result. See the diagram below.

Fig.9 - Distance Calculation
Face Tracking
High-Level Commands
Fig.10 - Joint Detail
Fig.11 - Pan-Tilt Detail

With the distance of the largest face bounding box to the center of the image frame, it is possible to implement a controller for face tracking. The most sophisticated way of doing this is through a 3D representation of the face coordinates by calculating its transform from the robot base coordinates, then planning a path using ROS. However, as mentioned earlier, due to computation restrictions in addition to the heavy load introduced by the computer vision algorithm, this was not practical to implement. Thus, we have simplified the problem to a two joint pan-tilt camera tracking problem.

Under this simplification, the two joints are independent and control separate axis of displacement in the camera frame. For the horizontal motion, joint 1 is used with a proportional velocity controller. For the vertical motion, joint 3 is used with a pseudo-velocity proportional controller. See the diagram below.

For joint 1, since gravity is perpendicular to it, no complicated control law is needed for good results. It is under no varying load, except for friction that can effectively be ignored. Thus, the x offset of the face detection box is fed straight into the controller, as the motor power input. Since the offset is signed, this allows for the controller to work in both directions. Thus, the closer the face is to the center of the frame, the weaker the control input will be, and vice versa. The joint 1 servo is set to motor mode so that it turns with the power input parameter. This allows the robot to horizontally converge to the face, putting it on the image frame’s centerline.

For joint 3, however, gravity is in the same direction as the direction of travel, thus a more sophisticated controller is required. However, instead of programming and testing a PID loop, which can be difficult and time-consuming, we opted to use the servo’s built-in position controller which already performs well. From that, we developed what we call a “pseudo-velocity” proportional controller. With a vertical face offset, depending on the sign of the offset, the control loop either increments or decrements an accumulation variable. Then, if the same sign of offset is persistent between loops, the accumulation continues to grow in magnitude. Joint 3’s angular position is read every loop, and the angular command is simply the joint position plus the accumulation variable. The accumulation is needed to act as an integral term, where if the last control angle value is not enough, it will keep incrementing in the right direction until the offset is negated. If the offset sign is flipped, the accumulation variable is immediately set to 0 and the control loop repeats. In testing, we found that although it does have some jitter and is not perfect, it functions well for tracking faces.

For both joints, in order to guard against unsafe joint angles, we have implemented software limits. Joint 1 is restricted to 50 to 900 in angle readings, and joint 3 is restricted to 150 to 900. If the lower limit is reached, the joint is not allowed to actuate further in the lower direction, while higher actuation values that will pull it back into the normal operating intervals are allowed. The same logic is set for the higher joint limits, and this system performed well to prevent physical collision and tangling of wires.

Code Integration
Fig.12 - Code Struture

We integrated the servo control and the face detection code by encapsulating separate scripts into different functions and placing them into a single script called face_servo.py. The structure of the final integrated code is organized in the following way: first are the encapsulated functions including randomizing movement function, light-switch function, and essential servo functions, and initialized variables including global control flag, a dictionary containing the pair of servo ids and original positions and the serial command dictionary. The second part is the main entry point which consists of the initialization of real-time video stream, looping over and processing the frames, doing face detection and calculating the center distance between the bounding box and the frame. Lastly, it is the servo control where we improved the code by constraining the possible movement range of the robot for safe operations. We put the random dance routine code at the end of the script as it would be blocking when the robot is dancing. It is worth noting that we programmed multi-threading code to achieve the light-switch routine since the robot continuously monitors the specific GPIO port defined manually. We initially used callback functions for the light switch, but was not able to get rid of false triggers reliably even with extremely long debounce times, so we wrote our own low state checking logic instead.

To achieve humanistic behaviors, we carefully developed the randomizing dance routine by setting random intervals between two routines to approximately 90-180 seconds. Specifically, we initialized a variable as the start time and use the Python time package to get the current time. By subtracting the initial time in every loop, we were able to retrieve the duration of each iteration. When the duration is greater than the defined showing routine timestamp, we called the corresponding function and simply generated a random number using the Python random package. With this random number, we choose a pre-recorded routine by its name and replay it in a for loop. When the display is finished, we reset the duration to 0 and the showing timestamp to another random number ranging from 90 to 180.

Testing

We did the hardware and software testing separately in order to simplify the debugging of potential problems. For mechanical hardware, most of the testing was done by assembling all the components in CAD, then moving them around and ensuring there was no collision. Since load was not a large factor here, physical packaging was the only concern here and the same testing was performed for the manufactured parts. For electrical hardware, testing was done by building the circuits and measuring the signals with an oscilloscope before connecting to the Pi. To debug the callback function issue, we looked at the trigger waveform on the scope but saw nothing abnormal. Thus, we decided it was not worth our time to debug further as it may be a library issue, so we just wrote our own multithreaded routine. The servos were rather straightforward as they are a commercial part, and required no debugging.

For software, the key problem to solve was the RPi camera, so we made sure it functioned as expected before starting any complex code. We ran some basic video streaming routines to ensure its functionality. Then we ran all the potentially helpful algorithms on our laptop. This way, we were able to understand the algorithms in-depth and quickly gain intuition about the performance of each algorithm. Then we adapted the algorithms into the Pi and visualized the captured frames and the detected bounding boxes using OpenCV. Furthermore, we printed the bounding boxes’ information in order to not only test the accuracy of our algorithms but also test the maximum frame rate. Despite all the challenges that we encountered, we managed to finish all the testing and achieved our initial objectives before the demo date and become one of the first groups to checkoff.

The first version of the integrated script worked well but the structure was not very readable, because we did not modularize each function and instead directly put them following the data flow order. Then we encountered problems when trying to modify some functions. So we realized that it was quite vulnerable to bugs and not user-friendly. We improved the structure by doing encapsulation, modulation, and multi-threading as stated in the code integration section. The individual functions of the arm were tested in separate scripts, and some progress snapshots can be seen in the videos below.

Testing Video

Results and Conclusions

We achieved our initial expectation where we designed and manufactured the 6-DOF robot arm and programmed it with sophisticated controls, developed efficient face detection algorithms and integrated them into a stable script.

For software, we achieved high frame-rate (20-30 fps) face detection for real-time facial tracking on the computation limited embedded system and safe servo control for showing various pre-recorded routines. After integrating the whole system, even though there was still a little jitter when tracking faces, the robot arm worked as expected.

Even though there remained inaccuracy of face detection due to the limited computational resources, our Pixar Lamp Robot met our objectives and was able to detect and track human faces. In addition, it was able to perform interesting postures randomly and then continue to do facial tracking. So we consider our project to be a great success. See the video below for a full demo.

Results Video

Future Plan

If we had more time to work on the project, we would like to explore using the Nvidia Jetson Nano for running a neural network for face detection. According to Nvidia forum users, it is capable of running the MTCNN architecture at around 10fps, leveraging the Maxwell GPU onboard for inference. Also, it would be interesting to train a custom lightweight network to run on both single-board computers and evaluate its accuracy vs. existing deep networks. Also, our current robot arm control design can be improved by using a fully non-deterministic algorithm generator, although that may require extensive effort to design.

Budget

Note that the robot arm cost was not included, as that was built for a different project.

Item Name Price Quantity
Raspberry Pi Camera V2 21 1
HDMI Adapter Board 14 1
Spiral HDMI Cable 9 1
LED Strip 0.5 1
Toggle Switch 0.5 1
M3 Screws 1 1
3D Printing Filament 20 1/2 Roll
Total $56

References

Contribution

  • Kowin Shi - Hardware Design & Integration, 3D Printing, CAD Models & Rendering
  • Tian Qiu - Software Control, Computer Vision, Website Building

Acknowledgements

  • Prof. Skovira - Advise on hardware, software, provider of RPi 3B+

Code

# import the necessary packages
        from imutils.video import VideoStream
        import argparse
        import imutils
        import time
        import cv2
        import os
        import numpy
        import random
        
        import serial
        import csv
        import RPi.GPIO as GPIO
        from threading import Thread
        
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(5, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
        
        # define global variable
        global joint_id
        global command
        global off_mode
        global light_on_sw
        global show_now
        show_now = False
        light_on_sw = False
        off_mode = True
        
        # initialize servo id and postion
        joint_id = {  #[real servo id, home position]
            "joint_1": [5, 852],
            "joint_2": [1, 597],
            "joint_3": [2, 135],
            "joint_4": [4, 510],
            "joint_5": [6, 900],
            "joint_6": [3, 474]
        }
        
        
        def show_routine(routine_file):
            # show robot dance routine
            # para:
            #   routine_file: a recorded CSV file
            global joint_id
            global show_now
            global light_on_sw
            global command
        
            if not light_on_sw:
                show_now = True
                init_time = time.time()
                duration = 0
        
                with open(routine_file) as csvfile:
                    pamreader = csv.reader(csvfile, delimiter=',')
                    for row in pamreader:
                        duration = time.time() - init_time
                        try:
                            newrow = list(map(float, row))
                        except Exception:
                            continue
                        rec_time = newrow[-1]
                        while (duration < rec_time):
                            duration = time.time() - init_time
                            time.sleep(0.0000001)
        
                        ind = 0
                        for key in joint_id.keys():
                            servoWriteCmd(joint_id[key][0],
                                          command["SERVO_MODE_WRITE"], 0)
                            servoWriteCmd(joint_id[key][0], command["MOVE_WRITE"],
                                          int(newrow[ind]), 0)
                            ind += 1
        
                for key in joint_id.keys():
                    servoWriteCmd(joint_id[key][0], command["SERVO_MODE_WRITE"], 0)
                    servoWriteCmd(joint_id[key][0], command["MOVE_WRITE"],
                                  joint_id[key][1], 0)
        
                show_now = False
        
        
        def GPIO5cb():
            # listen GPIO 5 and show light-switch routine
            # as long as the switch is off
            # two modes (fast mode and slow mode) for this routine
            global light_on_sw
            global joint_id
            global command
            global off_mode
            while True:
                time.sleep(1)
                if light_on_sw and not show_now:
        
                    if off_mode:
                        off_file = 'light_off.csv'
                    else:
                        off_file = 'light_off2.csv'
        
                    off_mode = not off_mode
        
                    for key in joint_id.keys():
                        servoWriteCmd(joint_id[key][0], command["SERVO_MODE_WRITE"], 0)
                        servoWriteCmd(joint_id[key][0], command["MOVE_WRITE"],
                                      joint_id[key][1], 0)
        
                    time.sleep(1.5)
        
                    init_time = time.time()
                    duration = 0
        
                    with open(off_file) as csvfile:
                        pamreader = csv.reader(csvfile, delimiter=',')
                        for row in pamreader:
                            duration = time.time() - init_time
                            try:
                                newrow = list(map(float, row))
                            except Exception:
                                continue
                            rec_time = newrow[-1]
                            while (duration < rec_time):
                                duration = time.time() - init_time
                                time.sleep(0.0000001)
        
                            ind = 0
                            for key in joint_id.keys():
                                servoWriteCmd(joint_id[key][0],
                                              command["SERVO_MODE_WRITE"], 0)
                                servoWriteCmd(joint_id[key][0], command["MOVE_WRITE"],
                                              int(newrow[ind]), 0)
                                ind += 1
        
                    togg = True
                    tog_speed = 1000
                    times = 0
        
                    while tog_speed >= 150:
                        time.sleep(0.03)
                        if togg:
                            servoWriteCmd(joint_id["joint_6"][0],
                                          command["SERVO_MODE_WRITE"], 1, tog_speed)
                        else:
                            servoWriteCmd(joint_id["joint_6"][0],
                                          command["SERVO_MODE_WRITE"], 1, -tog_speed)
                        togg = not togg
                        if times < 2:
                            times += 1
                        else:
                            tog_speed -= 150
                            times = 0
        
                    light_on_sw = False
        
                    servoWriteCmd(joint_id["joint_6"][0], command["SERVO_MODE_WRITE"],
                                  0)
                    servoWriteCmd(joint_id['joint_3'][0], command["MOVE_WRITE"], 250,
                                  0)
        
        
        # initialize and start the light-switch deamon
        light_off_thread = Thread(target=GPIO5cb, daemon=True)
        light_off_thread.daemon = True
        light_off_thread.start()
        
        # initialize serial and servo command
        serialHandle = serial.Serial("/dev/ttyUSB0", 115200)  #115200 baud rate
        
        command = {
            "MOVE_WRITE": 1,
            "POS_READ": 28,
            "SERVO_MODE_WRITE": 29,
            "LOAD_UNLOAD_WRITE": 31,
            "SERVO_MOVE_STOP": 12,
            "TEMP_READ": 26
        }
        
        
        #No need to split into higher and lower bytes, this function does it already. Parameter # is # of different params.
        def servoWriteCmd(id, cmd, par1=None, par2=None):
            # write commands to servos
            # para:
            #   id: servo id
            #   cmd: real command
            buf = bytearray(b'\x55\x55')
            try:
                len = 3  #length is 3 if no commands
                buf1 = bytearray(b'')
        
                ## verify data
                if par1 is not None:
                    len += 2  #add 2 to data length
                    buf1.extend([
                        (0xff & par1), (0xff & (par1 >> 8))
                    ])  #split into lower and higher bytes, store in buffer
                if par2 is not None:
                    len += 2
                    buf1.extend([
                        (0xff & par2), (0xff & (par2 >> 8))
                    ])  #split into lower and higher bytes, store in buffer
                buf.extend([(0xff & id), (0xff & len), (0xff & cmd)])
                buf.extend(buf1)
        
                ## checksum
                sum = 0x00
                for b in buf:  #sum
                    sum += b
                sum = sum - 0x55 - 0x55  #remove two beginning 0x55
                sum = ~sum  #take not
                buf.append(0xff & sum)  #add lower byte into buffer
                serialHandle.write(buf)  #send
            except Exception as e:
                print(e)
        
        
        def readPosition(id):
            # read the position of each servo
            # para:
            #   id: servo id
            serialHandle.flushInput()
            servoWriteCmd(id, command["POS_READ"])  #send read command
        
            time.sleep(0.0055)  #delay
        
            count = serialHandle.inWaiting()  #get number of bytes in serial buffer
            pos = None
            if count != 0:  #if not empty
                recv_data = serialHandle.read(count)  #read data
                if count == 8:  #if it matches expected data length
                    if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[
                            4] == 0x1C:
                        #first and second bytes are 0x55, fifth byte is 0x1C (28), which is read position command
                        pos = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8))
                                        )  #combine data for valid read
        
            return pos
        
        
        def readTemperature(id):
            # read the temperature of each servo
            # para:
            #   id: servo id
            serialHandle.flushInput()
            servoWriteCmd(id, command["TEMP_READ"])  #send read command
        
            time.sleep(0.01)  #delay
        
            count = serialHandle.inWaiting()  #get number of bytes in serial buffer
            tem = None
            if count != 0:  #if not empty
                recv_data = serialHandle.read(count)  #read data
                if count == 7:  #if it matches expected data length
                    if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[
                            4] == 0x1A:
                        #first and second bytes are 0x55, fifth byte is 0x1A (26), which is read temperature command
                        tem = recv_data[5]
        
            return tem
        
        
        # construct the argument parser and parse the arguments
        ap = argparse.ArgumentParser()
        ap.add_argument("-c",
                        "--cascade",
                        required=True,
                        help="path to where the face cascade resides")
        args = vars(ap.parse_args())
        
        # load OpenCV's Haar cascade for face detection from disk
        detector = cv2.CascadeClassifier(args["cascade"])
        
        # initialize the video stream, allow the camera sensor to warm up
        # the size of captured frames is set to width=320, height=240
        # frame per second is 32
        print("[INFO] starting video stream...")
        # vs = VideoStream(src=0).start()
        vs = VideoStream(usePiCamera=True, resolution=(320, 240), framerate=32).start()
        time.sleep(2.0)
        total = 0
        
        #servo control variables
        cont_var = 0
        last_input = True
        dist_center_x = 0
        hinc = 1
        linc = 1
        
        show_time = time.time()
        d_lim = 30
        t_duration = 0
        t_init = time.time()
        
        # loop over the frames from the video stream
        while True:
            # grab the frame from the threaded video stream and resize the frame
            # resize the frame size with fixed width/height ratio => width=400, height=300
            frame = vs.read()
            frame = imutils.resize(frame, width=400)
        
            # detect faces in the grayscale frame
            rects = detector.detectMultiScale(cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY),
                                              scaleFactor=1.1,
                                              minNeighbors=5,
                                              minSize=(60, 60))
        
            # camera center coordinates
            camera_center_x = 200
            camera_center_y = 150
        
            # loop over the face detections and control the servos
            # if the camera does detects some faces, then further
            # control the robot. Otherwise, it does not move
            if len(rects) > 0:
                # refine the bounding boxes by using only the maximum area bbx
                arr = numpy.zeros((len(rects), 1))
        
                for idx, (x, y, w, h) in enumerate(rects, start=1):
                    arr[idx - 1] = w * h
        
                max_ind = numpy.argmax(arr)
        
                x, y, w, h = rects[max_ind]
        
                # draw the bounding box on the original frame
                cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
                # the bounding box center coordinates
                bbx_center_x = x + w / 2
                bbx_center_y = y + h / 2
                # center distance
                dist_center_x = camera_center_x - bbx_center_x
                dist_center_y = camera_center_y - bbx_center_y
                print('---------------------------------------------------')
                print(
                    'Index: {} Camear center: {} Face center: {} Distance: {}'.format(
                        idx, (camera_center_x, camera_center_y),
                        (bbx_center_x, bbx_center_y), (dist_center_x, dist_center_y)))
        
                # read the position of joint 1 and 3
                pos = readPosition(joint_id['joint_1'][0])
                pos2 = readPosition(joint_id['joint_3'][0])
        
                if pos:
                    print('joint 1 position: {}'.format(pos))
                    # limit the movement angle for joint 1
                    if ((pos < 300) and (dist_center_x < 0)):
                        servoWriteCmd(joint_id['joint_1'][0],
                                      command["SERVO_MODE_WRITE"], 1,
                                      abs(int(2 * dist_center_x)))
                    elif ((pos > 900) and (dist_center_x > 0)):
                        servoWriteCmd(joint_id['joint_1'][0],
                                      command["SERVO_MODE_WRITE"], 1,
                                      -abs(int(2 * dist_center_x)))
                    else:
                        servoWriteCmd(joint_id['joint_1'][0],
                                      command["SERVO_MODE_WRITE"], 1,
                                      int(3 * dist_center_x))
        
                if pos2:
                    print('joint 3 position: {}'.format(pos2))
                    # limit the movement angle for joint 3
                    if pos2 > 150 :
                        if dist_center_y < 0:
                            if linc < 100:
                                linc += 1
                            hinc = 1
                            servoWriteCmd(joint_id['joint_3'][0],
                                          command["SERVO_MODE_WRITE"], 0)
                            servoWriteCmd(joint_id['joint_3'][0],
                                          command["MOVE_WRITE"], pos2 - linc, 0)
        
                    if pos2 < 900:
                        if dist_center_y > 0:
                            linc = 1
                            if hinc < 500:
                                hinc += 1
                            servoWriteCmd(joint_id['joint_3'][0],
                                          command["SERVO_MODE_WRITE"], 0)
                            servoWriteCmd(joint_id['joint_3'][0],
                                          command["MOVE_WRITE"], pos2 + hinc, 0)
                            print(pos2 + hinc)
            else:
                servoWriteCmd(joint_id['joint_1'][0], command["SERVO_MODE_WRITE"], 1,
                              0)
        
            # show dance routine randomly
            duration = time.time() - show_time
            if duration > d_lim:
                print('show')
                duration = 0
                show_time = time.time()
                d_lim = random.randint(90, 180)
                f_ind = random.randint(1, 7)
                show_routine('rand_rout' + str(f_ind) + '.csv')
        
            # show the output frame
            cv2.imshow("Frame", frame)
            key = cv2.waitKey(1) & 0xFF
        
            t_duration = time.time() - t_init
        
            if t_duration > 5:
                t_init = time.time()
                tem_out = []
                for key in joint_id.keys():
                    tem_out.append(readTemperature(joint_id[key][0]))
        
                print(tem_out)
        
            now_input = GPIO.input(5)
            if not now_input and not last_input:
                cont_var += 1
            else:
                cont_var = 0
        
            if cont_var > 6:
                cont_var = -10
                light_on_sw = True
        
            last_input = now_input
        
        # do a bit of cleanup
        print("[INFO] {} face images stored".format(total))
        print("[INFO] cleaning up...")
        cv2.destroyAllWindows()
        vs.stop()