def main(args): rospy.init_node('GummiConfigure', anonymous=True) r = rospy.Rate(60) gummi = Gummi() rospy.logwarn( 'WARNING: Moving joints sequentially to zero equilibrium and zero co-contraction for antagonist joints, zero joint angle for direct drive joints.' ) rospy.sleep(2) gummi.doZeroAllServos() rospy.sleep(2) rospy.logwarn('Done.') r.sleep()
def main(args): pi = 3.1416 rospy.init_node('gummiCalibrate', anonymous=True) r = rospy.Rate(60) gummi = Gummi() name = raw_input("Enter name of antagonist joint to test: ") joint = gummi.joints[name]['controller'] path = raw_input( "Please enter path to folder where you want calibration file saved: ") numSteps = 9 minAngle = joint.angle.getMin() maxAngle = joint.angle.getMax() print("Using min angle: " + str(minAngle) + ", max angle: " + str(maxAngle) + ", and steps: " + str(numSteps)) anglesToTry = np.linspace(minAngle, maxAngle, numSteps) cocontractionsToTry = np.linspace(1.0, 0.0, numSteps) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() for i in range(0, 100): #gummi.forearmRoll.servoTo(-pi/2) r.sleep() print("GummiArm is live!") for i in range(0, 300): joint.servoTo(0, 0.5) r.sleep() thetas = list() ccs = list() equilibriums = list() for cocont in cocontractionsToTry: for i in range(0, 350): joint.servoTo(0, cocont) r.sleep() for angle in anglesToTry: print("Moving joint to angle: " + str(angle) + " and cocontraction: " + str(cocont) + ".") for i in range(0, 350): joint.servoTo(angle, cocont) r.sleep() thetas.append(round(joint.angle.getEncoder(), 3)) ccs.append(round(cocont, 3)) equilibriums.append(round(joint.eqModel.dEquilibrium, 3)) for i in range(0, 350): joint.servoTo(0, cocont) r.sleep() data = {'thetas': thetas, 'ccs': ccs, 'equilibriums': equilibriums} text = yaml.dump(data, default_flow_style=False, explicit_start=True) print text fileName = path + "/calibration_" + joint.getName() + ".yaml" with open(fileName, 'w') as outfile: outfile.write(text) print("Calibration data written to: " + fileName) jm = InverseModel("test") jm.setCalibration(thetas, ccs, equilibriums) gridX, gridY = np.mgrid[minAngle:maxAngle:0.01, 0:1:0.01] map = jm.getMap(gridX, gridY) plt.imshow(map.T, extent=(minAngle, maxAngle, 0, 1)) plt.show()
def main(args): rospy.init_node('GummiArm', anonymous=False) # anonymous=False because doesn't look like a good idea two GummiArm nodes... r = rospy.Rate(60) # Tried 100 and rostopic hz said it was working! # With 600 the elbow PID controller went crazy! gummi = Gummi() rospy.logwarn('Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() rospy.logwarn('Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() #for i in range(0,100): # gummi.forearmRoll.servoTo(pi/2) # r.sleep() gummi.setCollisionResponses(shoulder_yaw=False, shoulder_roll=False, shoulder_pitch=False, elbow=False) rospy.loginfo("GummiArm is live!") while not rospy.is_shutdown(): try: rospy.has_param("/dynamixel_manager_arm/namespace") # rospy.has_param("/dynamixel_manager_arm/namespace") is a way to verify if the manager is running if gummi.teleop == 0 and gummi.velocity_control == 0: gummi.doUpdate() gummi.publishJointState() r.sleep() except: rospy.signal_shutdown( "gummi_dynamixel manager seems to be dead... exiting!")
def main(args): pi = 3.1416 cocontractionsToTry = (0.0, 0.25, 0.5, 0.75, 1.0) rospy.init_node('gummi', anonymous=True) r = rospy.Rate(60) gummi = Gummi() numAttempts = rospy.get_param("~numAttempts") path = rospy.get_param("~outputPath") jointNums = rospy.get_param("~jointNums") elbowExtended = rospy.get_param("~elbowExtended") for jointNum in jointNums: print("Output path is '" + path + "'.") print("Joint to test: " + str(jointNum) + ".") print("Elbow extended: " + str(elbowExtended) + ".") if jointNum is 0: joint = gummi.shoulderYaw else: if jointNum is 1: joint = gummi.shoulderRoll else: if jointNum is 2: joint = gummi.shoulderPitch else: if jointNum is 4: joint = gummi.elbow else: if jointNum is 6: joint = gummi.wrist minAngle = joint.angle.getMin() * 180 / pi maxAngle = joint.angle.getMax() * 180 / pi rangeAngle = maxAngle - minAngle rest = minAngle + rangeAngle / 4 desired = maxAngle - rangeAngle / 4 print("Moving from rest: " + str(rest) + ", to desired: " + str(desired) + ".") gummi.setCocontraction(0.8, 0.8, 0.8, 0.8, 0.8) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() print("GummiArm is live!") print("Moving arm into place.") joint.goTo(rest * pi / 180, 0.3, True) for i in range(0, 200): joint.goTo(rest * pi / 180, 0.3, False) if elbowExtended: gummi.elbow.servoTo(gummi.elbow.angle.getMin(), 0.8) r.sleep() for cocont in cocontractionsToTry: for att in range(1, numAttempts + 1): print("Setting desired cocontraction.") for i in range(0, 600): joint.servoTo(rest * pi / 180, cocont) if elbowExtended: gummi.elbow.servoTo(gummi.elbow.angle.getMin(), 0.8) r.sleep() print("Test started, cocontraction: " + str(cocont) + ", attempt: " + str(att) + ".") fileName = path + '/step_test_' + joint.getName( ) + '_s_' + str(cocont) + '_a_' + str(att) + '.csv' with open(fileName, 'wb') as csvfile: writer = csv.writer(csvfile, delimiter=' ', quotechar='|', quoting=csv.QUOTE_MINIMAL) writer.writerow([ 'time', 'desired', 'angle', 'equilibrium', 'cocontraction', 'flexor', 'extensor', 'ballistic_reflex' ]) time1 = rospy.Time.now() now = False for i in range(0, 1400): if i < 200: command = rest joint.servoTo(command * pi / 180, cocont) else: if i < 600: command = desired now = False if i == 200: now = True else: command = rest now = False if i == 600: now = True #joint.servoTo(command * pi/180, cocont) joint.goTo(command * pi / 180, cocont, now) angle = joint.getJointAngle() * 180 / pi time2 = rospy.Time.now() duration = time2 - time1 delta = duration.to_sec() equilibrium = joint.eqModel.dEquilibrium cocontraction = joint.eqModel.cCocontraction flexor = joint.eqModel.flexor.getJointAngle( ) * 180 / pi extensor = joint.eqModel.extensor.getJointAngle( ) * 180 / pi ballistic = joint.ballistic writer.writerow([ delta, command, angle, equilibrium, cocontraction, flexor, extensor, ballistic ]) r.sleep()
def main(args): pi = 3.1416 rospy.init_node('gummi', anonymous=True) r = rospy.Rate(60) cocont = 0.25 #desired = (0.77, 0.69, 0.64, 0.72, 0.10, -0.05, 0.01) #desired = (0.0, 0.9, 0.25, 0.0, -0.2, 0.0, 0.01) #desired = (0.0, 0.9, 0.3, 0.0, 0.15, 0.0, 0.01) desired = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) gummi = Gummi() print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(3) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() print("GummiArm is live!") gummi.setCocontraction(cocont, cocont, cocont, cocont, 0.4) print("Moving arm into place.") gummi.goTo(desired, True) for i in range(0, 400): gummi.goTo(desired, False) r.sleep() print("Setting co-contraction - hold arm.") for i in range(0, 200): gummi.servoTo(desired) r.sleep() print("Test started, cocontraction: " + str(cocont) + ".") while True: gummi.passiveHold() r.sleep()
def main(args): pi = 3.1416 cocontractionsToTry = (0.25, 0.25) jointNums = (0,1,2,4,6) rospy.init_node('gummi', anonymous=True) r = rospy.Rate(60) gummi = Gummi() gummi.setCocontraction(0.8, 0.8, 0.8, 0.8, 0.8) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(3) gummi.goRestingPose(True) for i in range(0,400): gummi.goRestingPose(False) r.sleep() print("GummiArm is live!") for jointNum in jointNums: if jointNum is 0: joint = gummi.shoulderYaw else: if jointNum is 1: joint = gummi.shoulderRoll else: if jointNum is 2: joint = gummi.shoulderPitch else: if jointNum is 4: joint = gummi.elbow else: if jointNum is 6: joint = gummi.wrist minAngle = joint.angle.getMin()*180/pi maxAngle = joint.angle.getMax()*180/pi rangeAngle = maxAngle - minAngle rest = minAngle + rangeAngle/4 desired = maxAngle - rangeAngle/4 for cocont in cocontractionsToTry: for i in range (0,500): if i < 200: command = rest joint.servoTo(command * pi/180, cocont) else: if i < 400: command = desired now = False if i == 200: now = True else: command = rest now = False if i == 400: now = True joint.goTo(command * pi/180, cocont, now) r.sleep() for i in range(0,400): gummi.goRestingPose(False) r.sleep()
def main(args): rospy.init_node('GummiArm', anonymous=True) r = rospy.Rate(60) gummi = Gummi() print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0,400): gummi.goRestingPose(False) r.sleep() #for i in range(0,100): # gummi.forearmRoll.servoTo(pi/2) # r.sleep() gummi.setCollisionResponses(False, False, False, False, False) print("GummiArm is live!") while not rospy.is_shutdown(): if gummi.teleop == 0: gummi.doUpdate() gummi.publishJointState() r.sleep()
def main(args): rospy.init_node('GummiIdentify', anonymous=True) r = rospy.Rate(60) gummi = Gummi() gummi.setMaxLoads(100, 100, 100, 100, 100) gummi.setStiffness(0.5, 0.5, 0.5, 0.5, 0.5) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(3) for i in range(0, 400): gummi.goRestingPose() r.sleep() print("GummiArm is live!") velocities = [0, 0, 0, 0, 0, 0, 0] start = rospy.Time.now() ask = True while not rospy.is_shutdown(): if ask: indexes = [int(x) for x in raw_input("Enter index of joint to identify (1-7): ").split()] velocities = [0, 0, 0, 0, 0, 0, 0] index = indexes[0] - 1 start = rospy.Time.now() ask = False angles = gummi.getJointAngles() angles = radToDeg(angles) duration = rospy.Time.now() - start secondsSinceStart = duration.to_sec() if secondsSinceStart > 20: ask = True else: if secondsSinceStart > 15: print("Done, moving to resting pose.") gummi.goRestingPose() else: vel = 0.0 if secondsSinceStart > 5: vel = -0.01 else: vel = 0.01 print('At angle: ' + str(angles[index]) + ' degrees, sending velocity: ' + str(vel) + '.') velocities[index] = vel gummi.setVelocity(velocities) gummi.doUpdate() r.sleep()
def main(args): rospy.init_node('GummiArm', anonymous=False) # anonymous=False because doesn't look like a good idea two GummiArm nodes... r = rospy.Rate(40) # Tried 100 and rostopic hz said it was working! # With 600 the elbow PID controller went crazy! gummi = Gummi() rospy.logwarn( 'Moving joints sequentially to startup equilibrium positions.') gummi.doGradualStartup() # gummi.testLimits() rospy.logwarn('Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() gummi.setCollisionResponses(shoulder_yaw=False, shoulder_roll=False, shoulder_pitch=False, elbow=False) rospy.loginfo("GummiArm is live!") # name = ['shoulder_yaw','shoulder_roll','shoulder_pitch','upperarm_roll','elbow','forearm_roll','wrist_pitch'] # effort = [0.5,0.5,0.5,100,0.5,100,0.5] # class Msg: # def __init__(self, name, effort): # self.name = name # self.effort = effort # msgs = [] # for i, j in zip(name, effort): # msgs.append(Msg(i, j)) # raw_input('press enter') # gummi.setCocontraction(msgs) # gummi.servoTo() while not rospy.is_shutdown(): try: rospy.has_param("/dynamixel_manager_arm/namespace") # rospy.has_param("/dynamixel_manager_arm/namespace") is a way to verify if the manager is running if gummi.teleop == 0 and gummi.velocity_control == 0: gummi.doUpdate() gummi.publishJointState() r.sleep() except: rospy.signal_shutdown( "gummi dynamixel manager seems to be dead... exiting!")
def main(args): record = False if rospy.get_param("gummi/recording") == 1: record = True fileNameBase = rospy.get_param("gummi/name_base_recording") print("WARNING: Will record in files with base: " + fileNameBase + ".") rospy.init_node("GummiArm", anonymous=True) r = rospy.Rate(100) gummi = Gummi() gummi.setMaxLoads(10, 10, 10, 10, 10) print("WARNING: Moving joints sequentially to equilibrium positions.") gummi.doGradualStartup() print("WARNING: Moving to resting pose, hold arm!") rospy.sleep(3) for i in range(0, 400): gummi.goRestingPose() r.sleep() print("GummiArm is live!") if record: print("Recording!") gummi.prepareRecording(fileNameBase) gummi.startRecording() while not rospy.is_shutdown(): gummi.publishJointState() r.sleep()
def main(args): pi = 3.1416 rest = (-0.5,0.6632251157578453,0.5410520681182421,-0.12217304763960307,0.5585053606381855,-2.4085543677521746,-0.7330382858376184) mid = (0.05,1.8,0.2792526803190927,-0.17453292519943295,0.4014257279586958,-2.3736477827122884,-0.7330382858376184) desired = (0.1,1.8500490071139892,0.20943951023931956,-0.24434609527920614,-0.45,-2.7,-0.2617993877991494) rospy.init_node('gummi', anonymous=True) r = rospy.Rate(60) gummi = Gummi() gummi.setCocontraction(0.6, 0.6, 0.6, 0.6, 0.6) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(3) gummi.goRestingPose(True) for i in range(0,400): gummi.goRestingPose(False) r.sleep() for i in range(0,100): gummi.forearmRoll.servoTo(-pi/2) r.sleep() print("GummiArm is live!") while True: for i in range (0,500): gummi.setCocontraction(0.6, 0.6, 0.6, 0.6, 0.6) gummi.goTo(rest, False) r.sleep() for i in range (0,60): gummi.setCocontraction(0.6, 0.6, 0.85, 0.6, 0.2) gummi.goTo(mid, False) r.sleep() for i in range (0,500): gummi.setCocontraction(0.8, 0.5, 1.0, 0.3, 0.2) gummi.goTo(desired, True) r.sleep()
def main(args): rospy.init_node('GummiIdentify', anonymous=True) r = rospy.Rate(60) gummi = Gummi() gummi.setCocontraction(0.75, 0.75, 0.75, 0.95, 0.8) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() print("GummiArm is live!") velocities = [0, 0, 0, 0, 0, 0, 0] start = rospy.Time.now() ask = True while not rospy.is_shutdown(): if ask: indexes = [ int(x) for x in raw_input( "Enter index of joint to identify (1-7): ").split() ] velocities = [0, 0, 0, 0, 0, 0, 0] index = indexes[0] - 1 start = rospy.Time.now() ask = False angles = gummi.getJointAngles() angles = radToDeg(angles) duration = rospy.Time.now() - start secondsSinceStart = duration.to_sec() if secondsSinceStart > 20: ask = True else: if secondsSinceStart > 15: print("Done, moving to resting pose.") gummi.goRestingPose(False) else: vel = 0.0 if secondsSinceStart > 5: vel = -0.01 else: vel = 0.01 print('At angle: ' + str(angles[index]) + ' degrees, sending velocity: ' + str(vel) + '.') velocities[index] = vel gummi.setVelocity(velocities) gummi.doVelocityUpdate() r.sleep()
def main(args): pi = 3.1416 rest = (0.0, -0.34770231192074535, 0.03579288505066496, 0.0030679615757712823, -0.7465373167710121, -1.55, -0.0051132692929521375) mid = (0.0, 0.10737865515199489, 0.14317154020265985, -0.21475731030398976, -0.4755340442445488, -1.55, 0.0) desired = (0.3170226961630325, 0.5777994301035916, 0.22498384888989406, -0.2684466378799872, -0.3681553890925539, -1.25, 0.0) width = 1.2 #0.6 frequency = 4.5 rospy.init_node('gummi', anonymous=True) r = rospy.Rate(60) hand_shake = HandShake() gummi = Gummi() gummi.setCocontraction(0.6, 0.6, 0.6, 0.6, 0.6) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(1) for i in range(0, 400): gummi.goTo(rest, False) r.sleep() print("GummiArm is live!") do_shake_hand = False hand_is_closed = False time_counter = 1 while True: hand_shake.doUpdate() if do_shake_hand: if time_counter < 60: print "Moving, first step" gummi.setCocontraction(0.6, 0.6, 0.85, 0.6, 0.2) gummi.goTo(mid, False) r.sleep() else: if time_counter < 250: print "Moving, second step" gummi.setCocontraction(0.5, 0.4, 0.6, 0.55, 0.2) gummi.goTo(desired, False) # TODO: CALIB r.sleep() else: print "Waiting..." if time_counter < 1150: if not hand_is_closed: if hand_shake.havePersistentTouch(): print "Closing hand" gummi.handClose.servoTo(1.5) elbow_waiting = gummi.elbow.angle.getEncoder() hand_is_closed = True time_counter = 750 else: elbow = elbow_waiting + width / 2 * math.sin( frequency * time_counter / 60.0) gummi.elbow.servoTo(elbow, 0.3) else: if time_counter < 1350: print "Opening hand" gummi.handClose.servoTo(-2.2) hand_is_closed = False else: if time_counter < 1600: print "Go to rest" gummi.setCocontraction(0.6, 0.6, 0.6, 0.6, 0.6) gummi.goTo(rest, False) else: print "Done with hand shake" hand_shake.done() do_shake_hand = False time_counter = 0 time_counter = time_counter + 1 else: print "Passive hold" gummi.passiveHold() if hand_shake.haveNewPerson(): if hand_shake.havePersistentPerson(): do_shake_hand = True print "Do hand shake" r.sleep()
def main(args): pi = 3.1416 rospy.init_node('gummi', anonymous=True) r = rospy.Rate(60) cocontractionsToTry = (0.5, 1.0) #rest = (-0.13, -0.3, 0.89, -0.26, -0.05, 0.69, 0.6) #rest = (-0.29, -0.40, 0.68, -0.14, 0.14, -1.11, -0.39) rest = (-0.4264776001796058, -0.3632267925204951, 0.43952896154233645, -0.3519524936392377, 0.09959590942766063, -1.38566001295885, 0.1) #desired = (0.16, 0.56, 0.35, -0.56, 0.04, 0.40, -0.26) #desired = (0.44, 0.99, 0.06, -1.00, -0.56, 0.72, 0.43) #desired = (0.7998190768714568, 1.0201045071709505, 0.7797487277750484, -1.0531038996116087, -0.5453775677762133, 1.1723876722423197, 0.05333409312205947) desired = (0.8883467495266145, 1.3092029355656658, 0.24612332745751858, -1.4054250963362158, -0.6597164515897976, -0.2171647670527418, -0.08828321162619326) gummi = Gummi() gummi.setCocontraction(0.4, 0.4, 0.4, 0.4, 0.4) print('WARNING: Moving joints sequentially to equilibrium positions.') gummi.doGradualStartup() print('WARNING: Moving to resting pose, hold arm!') rospy.sleep(1) gummi.goRestingPose(True) for i in range(0, 400): gummi.goRestingPose(False) r.sleep() print("GummiArm is live!") for cocont in cocontractionsToTry: gummi.setCocontraction(cocont, cocont, cocont, cocont, cocont) for att in range(1, 2): print("Moving arm into place.") for i in range(0, 400): gummi.servoTo(rest) r.sleep() print("Test started, cocontraction: " + str(cocont) + ", attempt: " + str(att) + ".") time1 = rospy.Time.now() now = False for i in range(0, 1200): if i < 200: command = rest else: if i < 800: command = desired now = False if i == 600: print("GET READY!") if i == 200: now = True else: command = rest now = False if i == 800: now = True gummi.goTo(command, now) #gummi.servoTo(command) r.sleep()