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