robot_main_RPI.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Sun Mar 15 20:31:07 2020

@author: miguel-asd
"""

import numpy as np
import time
import csv
import sys
from simple_pid import PID 
import os

from src.kinematic_model import robotKinematics
from src.joystick import Joystick
from src.serial_com import ArduinoSerial
from src import angleToPulse
from src.gaitPlanner import trotGait
from src.CoM_stabilization import stabilize
from pathlib import Path

module_path=Path(os.path.join(Path(__file__).parent.parent,'turning-robot')).as_posix()

sys.path.append(module_path)
module_path=Path(os.path.join(Path(__file__).parent.parent,'turning-robot','scripts')).as_posix()
sys.path.append(module_path)
print(module_path)

import sys
from pathlib import Path
from robot_model.simulator import Simulator
import yaml
import gzip
import pickle
import yaml
from deap import algorithms, base, cma, creator, tools
from scoop import futures
from scripts.mo_evaluate import assign_individuals
import argparse
from robot_model.data_collector import SimulationData

def arg_parser():

    parser = argparse.ArgumentParser( prog = 'run_sim.py',
                        description = 'Runs the simulator with given parameters or optimization',)

    parser.add_argument('-y', "--yaml_file", default='data/mo_test_39/mo_test_39.yaml', dest='setup_file',
                        help='yaml file that contains the info')

    parser.add_argument('-o', "--opt_file", default="data/runs/mo_test_39_cp_04100.pickle.gzip", dest='opt_file',
                        help='yaml file that contains the info')
    args = parser.parse_args()
    return args

def set_leg_lengths(parameters):
    parameters['Simulator']['target_foot']['leg_length_bl']=[(parameters['Simulator']['target_foot']['leg_length'][0]),(parameters['Simulator']['target_foot']['leg_length'][1])]
    parameters['Simulator']['target_foot']['leg_length_br']=[(parameters['Simulator']['target_foot']['leg_length'][0]),(parameters['Simulator']['target_foot']['leg_length'][1])]
    parameters['Simulator']['target_foot']['leg_length_fl']=[(parameters['Simulator']['target_foot']['leg_length'][0]),(parameters['Simulator']['target_foot']['leg_length'][1])]
    parameters['Simulator']['target_foot']['leg_length_fr']=[(parameters['Simulator']['target_foot']['leg_length'][0]),(parameters['Simulator']['target_foot']['leg_length'][1])]
    parameters['Simulator']['Controller']['coupling_scaler']=1.0

##This part of code is just to save the raw telemetry data.
fieldnames = ["t",
              "FRc","FRf","FRt",
              "FLc","FLf","FLt",
              "BRc","BRf","BRt",     
              "BLc","BLf","BLt",
              "bodyAngle"]
#with open('telemetry/data.csv','w') as csv_file:
#    csv_writer = csv.DictWriter(csv_file,fieldnames = fieldnames)
#    csv_writer.writeheader()

def update_data(bodyAngle , angles , encoderAng):
    with open('telemetry/data.csv','a') as csv_file:
        csv_writer = csv.DictWriter(csv_file, fieldnames = fieldnames)
        info = {"t" :  round(t,2),
                "FRc" : round(encoderAng[0],1),"FRf" : round(encoderAng[1],1),"FRt" : round(encoderAng[2],1),
                "FLc" : round(encoderAng[3],1),"FLf" : round(angles[1,1],1),"FLt" : round(angles[1,2],1),
                "BRc" : round(angles[2,0],1),"BRf" : round(angles[2,1],1),"BRt" : round(angles[2,2],1),
                "BLc" : round(angles[3,0],1),"BLf" : round(angles[3,1],1),"BLt" : round(angles[3,2],1),
                "bodyAngle" : round(bodyAngle,1)}
        csv_writer.writerow(info)




            
def moveFeetTo(dt , bodytoFeet , newFeetPos , moveTime , h):
    #time in seconds to make the movement
    # h: step height in m.
    
    l1 = np.array([0,1])
    l2 = np.array([3,2])
    nit = int(moveTime/dt)
    for k in range(2):
        moveX1 = np.linspace(bodytoFeet[l1[k],0] , newFeetPos[l1[k],0] , nit)
        moveY1 = np.linspace(bodytoFeet[l1[k],1] , newFeetPos[l1[k],1] , nit)
        moveX2 = np.linspace(bodytoFeet[l2[k],0] , newFeetPos[l2[k],0] , nit)
        moveY2 = np.linspace(bodytoFeet[l2[k],1] , newFeetPos[l2[k],1] , nit)
        
#                     print(moveY)
        it = int(0)
        MVlastTime=0
        while it < nit:
            if (time.time()-MVlastTime >= dt):
                MVloopTime = time.time() - lastTime
                MVlastTime = time.time()
        
                bodyAngle , commandPose , commandOrn , V , angle , Wrot , T , compliantMode , kill , rest , poseMode = joystick.read()
                bodytoFeet[l1[k],0] = moveX1[it]
                bodytoFeet[l1[k],1] = moveY1[it]
                bodytoFeet[l1[k],2] = bodytoFeet0[l1[k],2] + ( 4*h*(it/nit-0.5)**2 - h )
                            
                bodytoFeet[l2[k],0] = moveX2[it]
                bodytoFeet[l2[k],1] = moveY2[it]
                bodytoFeet[l2[k],2] = bodytoFeet0[l2[k],2] + ( 4*h*(it/nit-0.5)**2 - h )
                
                #####################################################################################
                #####   kinematics Model: Input body orientation, deviation and foot position    ####
                #####   and get the angles, neccesary to reach that position, for every joint    ####
                angles , transformedBodytoFeet = robotKinematics.solve(bodyAngle, orn + commandOrn, pos + commandPose, bodytoFeet)
                pulsesCommand = angleToPulse.convert(angles, bodyAngle)
                arduino.serialSend(rest, CTRL1, CTRL2, en , pulsesCommand)#send serial command to arduino
                update_data(bodyAngle , angles , encoderAng)
                it = it + 1
                
def moveFeetZ(dt , fromHeight , toHeight , moveTime):
    nit = int(moveTime/dt) #time in sec to change feet pos.
    l1 = np.array([0,1])
    l2 = np.array([3,2])
    moveZ1 = np.linspace(fromHeight , toHeight , nit)
    moveZ2 = np.linspace(fromHeight , toHeight , nit)
    moveZ3 = np.linspace(fromHeight , toHeight , nit)
    moveZ4 = np.linspace(fromHeight , toHeight , nit)
    print('Standing up')
    it = int(0)
    MVlastTime=0
    while it < nit:
        if (time.time()-MVlastTime >= dt):
            MVloopTime = time.time() - lastTime
            MVlastTime = time.time()
            commandBodyAngle , commandPose , commandOrn , V , angle , Wrot , T , compliantMode , kill , rest , poseMode = joystick.read()
            
            bodytoFeet[0,2] = moveZ1[it]
            bodytoFeet[1,2] = moveZ2[it]
            bodytoFeet[2,2] = moveZ3[it]
            bodytoFeet[3,2] = moveZ4[it]
            #####################################################################################
            #####   kinematics Model: Input body orientation, deviation and foot position    ####
            #####   and get the angles, neccesary to reach that position, for every joint    ####
            angles , transformedBodytoFeet = robotKinematics.solve(bodyAngle, orn, pos , bodytoFeet)
            pulsesCommand = angleToPulse.convert(angles, bodyAngle)
            arduino.serialSend(rest, CTRL1, CTRL2, en , pulsesCommand)#send serial command to arduino
            update_data(bodyAngle , angles , encoderAng)
            it = it + 1

def layDownMove(dt , bodyAngle , bodytoFeet , bodytoFeet0):
######            RESTING ROUTINE           ##################
    h = 0.06 # step height in m.
    
    print('Init REST')
    moveFeetTo(dt , bodytoFeet , bodytoFeet_REST , 0.3 , h)

    print('Going rest')
    moveFeetZ(dt , bodytoFeet[0,2] , 0.018 , 1.0)

def robotResting():
    bodytoFeet = bodytoFeet_REST.copy()
    
    angles , transformedBodytoFeet = robotKinematics.solve(bodyAngle, orn , pos , bodytoFeet)
    pulsesCommand = angleToPulse.convert(angles, bodyAngle)
    arduino.serialSend(rest, CTRL1, CTRL2, en , pulsesCommand)#send serial command to arduino
    update_data(bodyAngle , angles , encoderAng)
    
def standMove(dt , bodyAngle , bodytoFeet , bodytoFeet0):
    ######            STAND UP ROUTINE           ########################
    h = 0.06 # step height in m.

    print('Standing up')
    moveFeetZ(dt , 0.018 , bodytoFeet0[0,2] , 1.0)
                        
    print('Init pose 0')
    moveFeetTo(dt , bodytoFeet , bodytoFeet0 , 0.3 , h)

        

def exitProgram(bodyAngle , bodytoFeet):
    print("Reseting microcontroller and closing...")
    time.sleep(0.4)
    commandBodyAngle , commandPose , commandOrn , V , angle , Wrot , T , compliantMode , kill , rest , poseMode = joystick.read()    

    en = 1
    ####################################################################################
    #####   kinematics Model: Input body orientation, deviation and foot position    ####
    #####   and get the angles, neccesary to reach that position, for every joint    ####
    angles , transformedBodytoFeet = robotKinematics.solve(bodyAngle, orn + commandOrn, pos + commandPose, bodytoFeet)
    pulsesCommand = angleToPulse.convert(angles, bodyAngle)
    arduino.serialSend(rest, CTRL1, CTRL2, en , pulsesCommand)
    sys.exit()
#            time.sleep(8)
#            arduino.connect()
#            bodyAngle , commandPose , commandOrn , V , angle , Wrot , T , compliantMode , kill , poseMode = joystick.read()    
#            kill = False
#            en = 0


args=arg_parser()

with open(args.setup_file, 'r') as file:
    parameters = yaml.load(file, Loader=yaml.FullLoader)
    
if args.opt_file != False:
    toolbox = base.Toolbox()
    creator.create("FitnessMin", base.Fitness, weights=(-1.0,))
    creator.create("Individual", list, fitness=creator.FitnessMin)
    toolbox.register("map", futures.map)
    creator.create("Fit", base.Fitness, weights=(-1.0,-1.0,-1.0))
    creator.create("Individual", list, fitness=creator.Fit)
    toolbox.register("map", futures.map)
    with gzip.GzipFile(args.opt_file, "r") as cp_file:
        my_dict = pickle.load(cp_file)
    individuals=my_dict['strategy'].parents[7]
    #print(individuals)
    #individuals[9]=0.5
    #individuals[10]=0.1
    assign_individuals(individuals,parameters)
else:
    set_leg_lengths(parameters)
    
parameters = parameters["Simulator"]
parameters['debug']=True
parameters['target_foot']['connect']="GUI"
parameters['run_time']=100
#parameters['ramp_time']=100
sim=Simulator(parameters)


def run_sim():
    sim.params["optimize"] = False
    sim.params["record"] = False
    sim.params["real_time"] = False
    sim.do_simulation()
    sim.params["record"] = True
    sim.do_simulation()

    sim.target.stop()


run_sim()









robotKinematics = robotKinematics('><')
joystick = Joystick() #need to specify the event route
joystick.conect('/dev/input/event1')
#arduino = ArduinoSerial() #need to specify the serial port
#arduino.conect()
arduino = sim.target
trot = trotGait() 
control = stabilize()
#robot properties
"""initial safe position"""
#angles
targetAngs = np.array([0 , np.pi/4 , -np.pi/2, 0 ,#BR
                        0 , np.pi/4 , -np.pi/2, 0 ,#BL
                        0 , np.pi/4 , -np.pi/2, 0 ,#FL
                        0 , np.pi/4 , -np.pi/2, 0 ])#FR

#FR_0  to FR_4 
#FRcoord = np.matrix([0. , -3.6 , -0.15])
#FLcoord = np.matrix([0. ,  3.6 , -0.15])
#BRcoord = np.matrix([0. , -3.6 , -0.15])
#BLcoord = np.matrix([0. ,  3.6 , -0.15])


"initial foot position"
#foot separation (0.182 -> tetta=0) and distance to floor
Xdist = 0.22
Ydist = 0.14
height = 0.12
#body frame to foot frame vector (0.08/-0.11 , -0.07 , -height)
bodytoFeet0 = np.matrix([[ Xdist/2. , -Ydist/2. , height],
                         [ Xdist/2. ,  Ydist/2. , height],
                         [-Xdist/2. , -Ydist/2. , height],
                         [-Xdist/2. ,  Ydist/2. , height]])

bodytoFeet_REST = np.matrix([[ 0.124 , -0.1 , 0.018],
                             [ 0.124 ,  0.1 , 0.018],
                             [-0.124 , -0.1 , 0.018],
                             [-0.124 ,  0.1 , 0.018]])#0.018

orn = np.array([0. , 0. , 0.])
pos = np.array([0. , 0. , 0.])
Upid_yorn = [0.]
Upid_y = [0.]
Upid_xorn = [0.]
Upid_x = [0.]
startTime = time.time()
lastTime = startTime
t = []
en = 0        
T = 0.4 #period of time (in seconds) of every step
offset = np.array([0.5 , 0 , 0.0 , 0.5]) #defines the offset between each foot step in this order (FR,FL,BR,BL)
                                         # [0. , 0.25 , 0.75 , 0.5] creep gait
interval = 0.015 #loop time.
bodyAngle = 0.0
kill = False
rest = True
CTRL1 = False
CTRL2 = False
lr = True
bodytoFeet = bodytoFeet_REST.copy()

pid = PID(-0.05, 0.5, 0.0, setpoint=0.0)
pid.sample_time = interval
pid.output_limits = (np.deg2rad(-20) , np.deg2rad(20))

print('Loop t: ', interval , ', Feet pos: (X = 0.248 Y = 0.2 Z = 0.018) m , REST == True')
for k in range(100000000000):
    if (time.time()-lastTime >= interval):
        loopTime = time.time() - lastTime
        lastTime = time.time()
        t = time.time() - startTime
        commandBodyAngle , commandPose , commandOrn , V , angle , Wrot , T , compliantMode , kill , rest , poseMode = joystick.read()    
        if (kill == False):
            en = 0
            arduinoLoopTime , encoderAng = arduino.serialRecive()#recive serial message    |  , linearAcc , eulerAngles , angularVelocity 
        
            if (rest == True and lr == True):
                robotResting()
                
            elif (rest == False and lr == False):
                #calculates the feet coord for gait, defining length of the step and direction (0º -> forward; 180º -> backward)
                if (poseMode == False):
                    run_sim()
                    #bodytoFeet  = trot.loop(V , angle , Wrot , T , offset , bodytoFeet0)
                else:
                    bodytoFeet = bodytoFeet0.copy() # Static position.
#                     pos[0] = 0.005*np.sin(2*np.pi*t/2.)
#                     pos[1] = 0.005*np.cos(2*np.pi*t/2.)
                    #commandBodyAngle = - np.deg2rad(10)*np.cos(2*np.pi*t/T)
#                     commandBodyAngle = pid(eulerAngles[1])
#                    print(loopTime )
                    #commandPose[1] = -commandBodyAngle/20.
                    #####################################################################################
                    #####   kinematics Model: Input body orientation, deviation and foot position    ####
                    #####   and get the angles, neccesary to reach that position, for every joint    ####
                angles , transformedBodytoFeet = robotKinematics.solve(commandBodyAngle , orn + commandOrn, pos + commandPose, bodytoFeet)
                pulsesCommand = angleToPulse.convert(angles, commandBodyAngle)
                arduino.serialSend(rest, CTRL1, CTRL2, en , pulsesCommand)#send serial command to arduino
                update_data(commandBodyAngle , angles , encoderAng)
                
            elif (rest == True and lr == False):
                layDownMove(interval , bodyAngle , bodytoFeet , bodytoFeet0) 
                print('Loop t: ', interval , ', Feet pos: (X = 0.248 Y = 0.2 Z = 0.018) m , REST == True')
        
            elif (rest == False and lr == True):
                standMove(interval , bodyAngle , bodytoFeet , bodytoFeet0)
                print('Loop t: ', interval , ', Feet pos: (X = 0.248 Y = 0.1 Z = 0.120) m , REST == False')
                
            lr = rest
        else:
            exitProgram(bodyAngle , bodytoFeet)
  
#        Upid_x , Upid_y , errorX , errorY , Upid_xorn , Upid_yorn = control.centerPoint(realPitch , realRoll)
#        orn[0] = Upid_xorn
#        orn[1] = Upid_yorn
#        pos[0] = Upid_x
#        pos[1] = Upid_y
        print (loopTime , arduinoLoopTime , encoderAng)
       #        print (Wrot)