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()
