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