From 914558b2ccd7aa7081deab2bc7510b50531dc817 Mon Sep 17 00:00:00 2001 From: LabUser Date: Mon, 7 Aug 2017 11:12:28 -0400 Subject: [PATCH] origin-master --- IMU_Forearm_Control.py | 5 +- IMU_Shoulder_Control.py | 182 ++++++++++++++++++++++++++++++++++++++++ No_Baxter.py | 150 +++++++++++++++++++++++++++++++++ SerialTest.py | 58 +++++++++++++ 4 files changed, 394 insertions(+), 1 deletion(-) create mode 100644 IMU_Shoulder_Control.py create mode 100644 No_Baxter.py create mode 100644 SerialTest.py diff --git a/IMU_Forearm_Control.py b/IMU_Forearm_Control.py index cebfca0..416941a 100644 --- a/IMU_Forearm_Control.py +++ b/IMU_Forearm_Control.py @@ -17,8 +17,10 @@ def forearm_control(): + right = baxter_interface.Limb('right') rj = right.joint_names() + right.set_joint_position_speed(1.0) offsetp = calibrate(1) while True: sensorValues = ser.readline() @@ -29,7 +31,8 @@ def forearm_control(): roll = numpy.radians(float(ypr[2])) print roll #print(str(yaw) + ", " + str(pitch) + ", " + str(roll)) - right.set_joint_positions({rj[4]:roll}) + pos = ({rj[4]:roll, rj[3]:pitch}) + right.set_joint_positions(pos) #right.set_joint_positions({rj[6]:roll}) def is_float(s): diff --git a/IMU_Shoulder_Control.py b/IMU_Shoulder_Control.py new file mode 100644 index 0000000..2cd23ac --- /dev/null +++ b/IMU_Shoulder_Control.py @@ -0,0 +1,182 @@ +import argparse +import numpy +import time +import rospy + +import baxter_interface +import baxter_external_devices + +from baxter_interface import CHECK_VERSION +import serial + + + +ser = serial.Serial('/dev/ttyACM0', 9600) +y1 = 0 +p1 = 1 +r1 = 2 +y2 = 3 +p2 = 4 +r2 = 5 + + +def forearm_control(): + right = baxter_interface.Limb('right') + rj = right.joint_names() + offsets = offset() + + while True: + sensorValues = ser.readline() + if(is_float(sensorValues)): + yprtot = ypr_calc(offsets) + set_j(yprtot) + #print(str(yaw) + ", " + str(pitch) + ", " + str(roll)) + + +#yaw = numpy.radians(float(ypr[0])) - offsety +#pitch = numpy.radians(float(ypr[1])) - offsetp +#roll = numpy.radians(float(ypr[2])) - offsetr + + +def ypr_calc(offsets): + + msg = recvFromArduino() + + yprsnew = msg.split() + yprs = [float(i) for i in yprsnew] + + + yprs1 = [yprs[0]-offsets[0], yprs[1]-offsets[1], yprs[2]-offsets[2]] + yprs2 = [yprs[3]-offsets[3], yprs[4]-offsets[4], yprs[5]-offsets[5]] + yprs2 = list(numpy.array(yprs2)-numpy.array(yprs1)) + yprtot = [yprs1, yprs2] + print(yprtot) + return yprtot + + + +#**************** + + +def joint_finder(yprtot): + if not yprtot[0]: + ypr = [rj[0], rj[1], rj[2]] + else: + ypr = [rj[3], rj[4]] + return ypr + + +#**************** + + +def is_float(s): + try: + float(s.split()[0]) + float(s.split()[1]) + float(s.split()[2]) + float(s.split()[3]) + float(s.split()[4]) + float(s.split()[5]) + float(s.split()[6]) + float(s.split()[7]) + + return True + except ValueError: + return False + except IndexError: + return False + +#**************** + +def calibrate(s): + sensorValues = ser.readline() + print sensorValues + print("SensorValues.split:" ) + print sensorValues.split() + return float(sensorValues.split()[s]) + + +#**************** + + +def offset(): + time.sleep(10) + sensorValues = ser.readline() + offsets = [] + offsets.append(calibrate(y1)) + offsets.append(calibrate(p1)) + offsets.append(calibrate(r1)) + offsets.append(calibrate(y2)) + offsets.append(calibrate(p2)) + offsets.append(calibrate(r2)) + return offsets + +def set_j(yprtot): + right = baxter_interface.Limb('right') + + rj = right.joint_names() + yprs1 = yprtot[0] + yprs2 = yprtot[1] + joint_command = {rj[0]:yprs1[0], rj[1]:yprs1[1], rj[2]:yprs1[2], rj[3]:yprs2[1], rj[4]:yprs2[2] } + right.set_joint_positions((joint_command)) + + +def recvFromArduino(): + global startMarker, endMarker + + ck = "" + x = "z" # any value that is not an end- or startMarker + byteCount = -1 # to allow for the fact that the last increment will be one too many + + # wait for the start character + while ord(x) != startMarker: + x = ser.read() + + # save data until the end marker is found + while ord(x) != endMarker: + if ord(x) != startMarker: + ck = ck + x + byteCount += 1 + x = ser.read() + + return(ck) + + +#**************************************************************************************************** + + +def main(): + """PROGRAM TO CONTROL THE WRIST PITCH AND ROLL FROM IMU ON HUMAN HAND """ + + + epilog = """ +See help inside the example with the '?' key for key bindings. + """ + arg_fmt = argparse.RawDescriptionHelpFormatter + parser = argparse.ArgumentParser(formatter_class=arg_fmt, + description=main.__doc__, + epilog=epilog) + parser.parse_args(rospy.myargv()[1:]) + + print("Initializing node... ") + rospy.init_node("rsdk_joint_angle_test") + print("Getting robot state... ") + rs = baxter_interface.RobotEnable(CHECK_VERSION) + init_state = rs.state().enabled + def clean_shutdown(): + print("\nExiting example...") + if not init_state: + print("Disabling robot...") + rs.disable() + + + rospy.on_shutdown(clean_shutdown) + + print("Enabling robot... ") + rs.enable() + forearm_control() + print("Done.") + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/No_Baxter.py b/No_Baxter.py new file mode 100644 index 0000000..0de7a84 --- /dev/null +++ b/No_Baxter.py @@ -0,0 +1,150 @@ +import argparse +import numpy +import time + +import serial + + + +ser = serial.Serial('/dev/ttyACM0', 9600) + + +y1 = 0 #\ +p1 = 1 # \ +r1 = 2 # \ Each of the numbers corresponds to each of the roll, pitch, and yaw positions in the recieved lists +y2 = 3 # / +p2 = 4 # / +r2 = 5 #/ + + +#right = baxter_interface.Limb('right') +#rj = right.joint_names() + + #set_j(yprtot) + #print(str(yaw) + ", " + str(pitch) + ", " + str(roll)) + + +#yaw = numpy.radians(float(ypr[0])) - offsety +#pitch = numpy.radians(float(ypr[1])) - offsetp +#roll = numpy.radians(float(ypr[2])) - offsetr + + +def offsetter(num): #Creates offsets in order for calibration and "zeroing" of baxter arm + sensorValues = recvFromArduino() + counter = 0 + offsets = [] + while(not is_float(sensorValues)): + sensorValues = recvFromArduino() + + while(is_float(sensorValues)): + if counter == 1000: + offsets = [] + offsets.append(calibrate(y1)) + offsets.append(calibrate(p1)) + offsets.append(calibrate(r1)) + offsets.append(calibrate(y2)) + offsets.append(calibrate(p2)) + offsets.append(calibrate(r2)) + print(offsets) + return offsets + else: + counter = counter+ 1 + sensorValues = recvFromArduino() + +def ypr_calc(offsets): # Calculates each of the yaw, pitch, and roll values from the arduino + msg = recvFromArduino() + #print msg.split() # and organizes them into a readable format to send to Baxter + if is_float(msg) == True: + yprsnew = msg.split() + yprs = [float(i) for i in yprsnew] + yprs1 = [yprs[0]-offsets[0], yprs[1]-offsets[1], yprs[2]-offsets[2]] # Subtracts the offsets from the recieved values + yprs2 = [yprs[3]-offsets[3], yprs[4]-offsets[4], yprs[5]-offsets[5]] # " " + yprs3 = list(numpy.array(yprs2)-numpy.array(yprs1)) # Subtracts the values from the second IMU from the first, in order to avoid any double movements + yprtot = [yprs1, yprs3] + #print(yprtot) + return yprtot + + + +#**************** + + +def joint_finder(yprtot): + if not yprtot[0]: + ypr = [rj[0], rj[1], rj[2]] + else: + ypr = [rj[3], rj[4]] + return ypr + + +#**************** + + +def is_float(s): + try: + float(s.split()[0]) + float(s.split()[1]) + float(s.split()[2]) + float(s.split()[3]) + float(s.split()[4]) + float(s.split()[5]) + + + return True + except ValueError: + return False + except IndexError: + return False + +#**************** + +def calibrate(s): + sensorValues = recvFromArduino() + #print("SensorValues.split:" ) + return float(sensorValues.split()[s]) + + +#**************** + + + +def set_j(yprtot): + right = baxter_interface.Limb('right') + + rj = right.joint_names() + yprs1 = yprtot[0] + yprs2 = yprtot[1] + joint_command = {rj[0]:yprs1[0], rj[1]:yprs1[1], rj[2]:yprs1[2], rj[3]:yprs2[1], rj[4]:yprs2[2] } + right.set_joint_positions((joint_command)) + + +def recvFromArduino(): + global startMarker, endMarker + + ck = "" + x = "z" # any value that is not an end- or startMarker + byteCount = -1 # to allow for the fact that the last increment will be one too many + + # wait for the start character + while ord(x) != startMarker: + x = ser.read() + + # save data until the end marker is found + while ord(x) != endMarker: + if ord(x) != startMarker: + ck = ck + x + byteCount += 1 + x = ser.read() + + return(ck) + + +#**************************************************************************************************** +startMarker = 60 +endMarker = 62 + +offsets = list() +offsets = offsetter(2) +while True: + yprtot = ypr_calc(offsets) + print(yprtot) \ No newline at end of file diff --git a/SerialTest.py b/SerialTest.py new file mode 100644 index 0000000..a413095 --- /dev/null +++ b/SerialTest.py @@ -0,0 +1,58 @@ +import argparse +import numpy +import time +import serial +import struct +import binascii + +ser = serial.Serial('/dev/ttyACM0', 9600) + + + + +def waitForArduino(): + + # wait until the Arduino sends 'Arduino Ready' - allows time for Arduino reset + # it also ensures that any bytes left over from a previous message are discarded + + global startMarker, endMarker + + msg = "" + while msg.find("Arduino is ready") == -1: + + while ser.inWaiting() == 0: + pass + + msg = recvFromArduino() + + print msg + print + +def recvFromArduino(): + global startMarker, endMarker + + ck = "" + x = "z" # any value that is not an end- or startMarker + byteCount = -1 # to allow for the fact that the last increment will be one too many + + # wait for the start character + while ord(x) != startMarker: + x = ser.read() + + # save data until the end marker is found + while ord(x) != endMarker: + if ord(x) != startMarker: + ck = ck + x + byteCount += 1 + x = ser.read() + + return(ck) + + +startMarker = 60 +endMarker = 62 +time.sleep(2) +while True: + + message = recvFromArduino() + print message