def initJoints(self):
self.shoulderYaw = Antagonist("shoulder_yaw")
self.shoulderRoll = Antagonist("shoulder_roll")
self.shoulderPitch = Antagonist("shoulder_pitch")
self.upperarmRoll = DirectDrive("upperarm_roll", self.pi)
self.elbow = Antagonist("elbow")
self.forearmRoll = DirectDrive("forearm_roll", 1.75*self.pi)
self.wrist = Antagonist("wrist")
self.headYaw = DirectDrive("head_yaw", 1.5*self.pi)
def initJoints(self):
for name in self.joints.keys():
if self.joints[name]['antagonist']:
self.joints[name]['controller'] = Antagonist(name)
else:
self.joints[name]['controller'] = DirectDrive(name)
self.joints[name]['position'] = self.joints[name][
'controller'].getJointAngle()
self.joints[name]['velocity'] = self.joints[name][
'controller'].getJointVelocity()
def main(args):
pi = 3.1416
equilibriums = (-0.75, -0.5)
stiffnesses = (0, 0.25, 0.5, 0.75, 1)
distances = range(7,21,1)
with open('results/stiffness_test.csv', 'wb') as csvfile:
writer = csv.writer(csvfile, delimiter=' ', quotechar='|', quoting=csv.QUOTE_MINIMAL)
writer.writerow(['attempt', 'distance', 'stiffness', 'equilibirum', 'angle'])
rospy.init_node('stiffnessTest', anonymous=True)
upperarmRoll = DirectDrive("upperarm_roll", pi)
elbow = Antagonist(-1, 1, -1, -1, 1, "biceps", "triceps", "elbow_encoder", 0, 2*pi, -0.75, 0.85)
r = rospy.Rate(60)
print("Moving arm into place.")
for i in range (0,300):
upperarmRoll.servoTo(0)
elbow.moveTo(0, 0)
r.sleep()
print("Test starting. Add weights.")
for dist in distances:
try:
input("Adjust distance to " + str(dist) + " cm, and press enter.")
except SyntaxError:
pass
for att in range (1,4):
for stiff in stiffnesses:
for eq in equilibriums:
for i in range (0,200):
upperarmRoll.servoTo(0)
elbow.moveTo(eq, stiff)
r.sleep()
angle = elbow.getJointAngle()
writer.writerow([att, dist, stiff, eq, angle])
def main(args):
pi = 3.1416
rospy.init_node('stiffnessTest', anonymous=True)
upperarmRoll = DirectDrive("upperarm_roll", pi)
elbow = Antagonist(-1, 1, -1, -1, 1, "biceps", "triceps", "elbow_encoder",
0, 2 * pi, -0.75, 0.85)
forearmRoll = DirectDrive("forearm_roll", 1.75 * pi)
wrist = Antagonist(-1, -1, -1, -1, 1, "wrist_flexor", "wrist_extensor",
"wrist_encoder", 0, 1.75 * pi, -1.0, 1.0)
r = rospy.Rate(60)
for att in range(1, 11):
print("Moving arm into place.")
for i in range(0, 150):
upperarmRoll.servoTo(0)
forearmRoll.servoTo(0)
wrist.servoTo(0, 0)
elbow.moveTo(0, 0)
r.sleep()
print("Test started, attempt: " + str(att) + ".")
fileName = 'results/step_test_stiffness_a_' + str(att) + '.csv'
with open(fileName, 'wb') as csvfile:
writer = csv.writer(csvfile,
delimiter=' ',
quotechar='|',
quoting=csv.QUOTE_MINIMAL)
writer.writerow([
'time', 'stiffness', 'angle', 'flexor-angle', 'extensor-angle'
])
time1 = rospy.Time.now()
for i in range(0, 400):
upperarmRoll.servoTo(0)
forearmRoll.servoTo(0)
wrist.servoTo(0, 0)
if i < 80:
stiff = 0.0
else:
if i < 240:
stiff = 1.0
else:
stiff = 0.0
elbow.moveTo(0, stiff)
angle = elbow.getJointAngle() * 180 / pi
flexor = joint.eqModel.flexor.getJointAngle() * 180 / pi
extensor = joint.eqModel.extensor.getJointAngle() * 180 / pi
time2 = rospy.Time.now()
duration = time2 - time1
delta = duration.to_sec()
writer.writerow(
[delta, stiff, angle, flexorAngle, extensorAngle])
r.sleep()
