class Gummi:
def __init__(self):
self.pi = 3.1416
self.initVariables()
self.initJoints()
self.initPublishers()
self.initSubscribers()
def initVariables(self):
self.jointNames = ("shoulder_yaw", "shoulder_roll", "shoulder_pitch", "upperarm_roll", "elbow", "forearm_roll", "wrist_pitch")
self.shoulderRollVel = 0
self.shoulderPitchVel = 0
self.shoulderYawVel = 0
self.upperarmRollVel = 0
self.elbowVel = 0
self.forearmRollVel = 0
self.wristVel = 0
self.shoulderRollStiff = 0
self.shoulderPitchStiff = 0
self.shoulderYawStiff = 0.3
self.elbowStiff = 0
self.wristStiff = 0
self.recordData = False
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 initPublishers(self):
self.jointStatePub = rospy.Publisher("gummi/joint_states", JointState, queue_size=10)
def initSubscribers(self):
rospy.Subscriber('gummi/joint_commands', JointState, self.cmdCallback)
def cmdCallback(self, msg):
self.setVelocity(msg.velocity) #TODO: CHECK NAMES
self.setStiffness(msg.effort[0], msg.effort[1], msg.effort[2], msg.effort[4], msg.effort[6])
self.doUpdate()
def setMaxLoads(self, maxLoadShoulderYaw, maxLoadShoulderRoll, maxLoadShoulderPitch, maxLoadElbow, maxloadWrist):
self.shoulderYaw.setMaxLoad(maxLoadShoulderYaw)
self.shoulderRoll.setMaxLoad(maxLoadShoulderRoll)
self.shoulderPitch.setMaxLoad(maxLoadShoulderPitch)
self.elbow.setMaxLoad(maxLoadElbow)
self.wrist.setMaxLoad(maxloadWrist)
def doUpdate(self):
if self.shoulderYawStiff < 0:
self.shoulderYaw.moveWith(self.shoulderYawVel, abs(self.shoulderYawStiff))
else:
self.shoulderYaw.servoWith(self.shoulderYawVel, self.shoulderYawStiff)
if self.shoulderRollStiff < 0:
self.shoulderRoll.moveWith(self.shoulderRollVel, abs(self.shoulderRollStiff))
else:
self.shoulderRoll.servoWith(self.shoulderRollVel, self.shoulderRollStiff)
if self.shoulderPitchStiff < 0:
self.shoulderPitch.moveWith(self.shoulderPitchVel, abs(self.shoulderPitchStiff))
else:
self.shoulderPitch.servoWith(self.shoulderPitchVel, self.shoulderPitchStiff)
self.upperarmRoll.servoWith(self.upperarmRollVel)
if self.elbowStiff < 0:
self.elbow.moveWith(self.elbowVel, abs(self.elbowStiff))
else:
self.elbow.servoWith(self.elbowVel, self.elbowStiff)
self.forearmRoll.servoWith(self.forearmRollVel)
if self.wristStiff == -999:
self.wrist.passiveHold(0.0)
else:
if self.wristStiff < 0:
self.wrist.moveWith(self.wristVel, abs(self.wristStiff))
else:
self.wrist.servoWith(self.wristVel, self.wristStiff)
self.publishJointState()
if self.recordData:
self.addLineRecording()
def publishJointState(self):
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.jointNames
msg.position = self.getJointAngles()
msg.effort = self.getLoads()
self.jointStatePub.publish(msg)
def getJointAngles(self):
angles = list()
angles.append(self.shoulderYaw.getJointAngle())
angles.append(self.shoulderRoll.getJointAngle())
angles.append(self.shoulderPitch.getJointAngle())
angles.append(self.upperarmRoll.getJointAngle())
angles.append(self.elbow.getJointAngle())
angles.append(self.forearmRoll.getJointAngle())
angles.append(self.wrist.getJointAngle())
return angles
def setVelocity(self, velocities):
self.shoulderYawVel = velocities[0]
self.shoulderRollVel = velocities[1]
self.shoulderPitchVel = velocities[2]
self.upperarmRollVel = velocities[3]
self.elbowVel = velocities[4]
self.forearmRollVel = velocities[5]
self.wristVel = velocities[6]
def setStiffness(self, shoulderYaw, shoulderRoll, shoulderPitch, elbow, wrist):
self.shoulderYawStiff = shoulderYaw
self.shoulderRollStiff = shoulderRoll
self.shoulderPitchStiff = shoulderPitch
self.elbowStiff = elbow
self.wristStiff = wrist
def getLoads(self):
loads = list()
loads.append(self.shoulderYaw.getLoadRatio())
loads.append(self.shoulderRoll.getLoadRatio())
loads.append(self.shoulderPitch.getLoadRatio())
loads.append(1234)
loads.append(self.elbow.getLoadRatio())
loads.append(1234)
loads.append(self.wrist.getLoadRatio())
return loads
def goRestingPose(self):
self.shoulderYaw.servoTo(0, self.shoulderYawStiff)
self.shoulderRoll.servoTo(0, self.shoulderRollStiff)
self.shoulderPitch.servoTo(0, self.shoulderPitchStiff)
self.upperarmRoll.servoTo(0)
self.elbow.servoTo(0, self.elbowStiff)
self.forearmRoll.servoTo(0)
self.wrist.servoTo(0, self.wristStiff)
def doGradualStartup(self):
self.shoulderYaw.moveTo(-0.1, self.shoulderYawStiff)
rospy.sleep(1)
self.shoulderRoll.moveTo(-1.25, self.shoulderRollStiff)
rospy.sleep(1)
self.shoulderPitch.moveTo(0.5, self.shoulderPitchStiff)
rospy.sleep(1)
self.upperarmRoll.servoTo(0)
rospy.sleep(1)
self.elbow.moveTo(0, self.elbowStiff)
rospy.sleep(1)
self.forearmRoll.servoTo(0)
rospy.sleep(1)
self.wrist.moveTo(0, self.wristStiff)
rospy.sleep(1)
self.headYaw.servoTo(0)
rospy.sleep(1)
def prepareRecording(self, fileNameBase):
self.shoulderYaw.prepareRecording(fileNameBase)
self.shoulderRoll.prepareRecording(fileNameBase)
self.shoulderPitch.prepareRecording(fileNameBase)
self.upperarmRoll.prepareRecording(fileNameBase)
self.elbow.prepareRecording(fileNameBase)
self.forearmRoll.prepareRecording(fileNameBase)
self.wrist.prepareRecording(fileNameBase)
def startRecording(self):
self.timeStartRecording = rospy.Time.now()
self.recordData = True
def stopRecording(self):
self.recordData = False
def addLineRecording(self):
timeNow = rospy.Time.now()
duration = timeNow - self.timeStartRecording
delta = duration.to_sec()
self.shoulderYaw.recordLine(delta)
self.shoulderRoll.recordLine(delta)
self.shoulderPitch.recordLine(delta)
self.upperarmRoll.recordLine(delta)
self.elbow.recordLine(delta)
self.forearmRoll.recordLine(delta)
self.wrist.recordLine(delta)
class Gummi:
def __init__(self):
self.teleop = rospy.get_param("~teleop", 1)
print(
"Expecting teleoperation ('teleop' parameter in gummi.yaml file): "
+ str(self.teleop) + ".")
self.pi = 3.1416
self.initVariables()
self.initJoints()
self.initPublishers()
self.initSubscribers()
def initVariables(self):
self.jointNames = ("shoulder_yaw", "shoulder_roll", "shoulder_pitch",
"upperarm_roll", "elbow", "forearm_roll",
"wrist_pitch", "hand_dof1")
self.shoulderRollVel = 0
self.shoulderPitchVel = 0
self.shoulderYawVel = 0
self.upperarmRollVel = 0
self.elbowVel = 0
self.forearmRollVel = 0
self.wristVel = 0
self.handDOF1Vel = 0
self.shoulderRollCocont = 0.3
self.shoulderPitchCocont = 0.3
self.shoulderYawCocont = 0.3
self.elbowCocont = 0.3
self.wristCocont = 0.6
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)
self.handDOF1 = DirectDrive("hand_dof1", 1.5 * self.pi)
def initPublishers(self):
self.jointStatePub = rospy.Publisher("gummi/joint_states",
JointState,
queue_size=10)
def initSubscribers(self):
rospy.Subscriber('gummi/joint_commands', JointState, self.cmdCallback)
def cmdCallback(self, msg):
if self.teleop == 1:
self.setVelocity(msg.velocity) #TODO: CHECK NAMES
self.setCocontraction(msg.effort[0], msg.effort[1], msg.effort[2],
msg.effort[4], msg.effort[6])
self.doVelocityUpdate()
else:
self.setCocontraction(abs(msg.effort[0]), abs(msg.effort[1]),
abs(msg.effort[2]), abs(msg.effort[4]),
abs(msg.effort[6]))
if (msg.effort[0] >= 0) and (msg.effort[1] >= 0) and (
msg.effort[2] >= 0) and (msg.effort[4] >=
0) and (msg.effort[6] >= 0):
self.servoTo(msg.position) #TODO: CHECK NAMES
else:
self.passiveHold()
def doUpdate(self):
self.shoulderYaw.doUpdate()
self.shoulderRoll.doUpdate()
self.shoulderPitch.doUpdate()
self.upperarmRoll.doUpdate()
self.elbow.doUpdate()
self.forearmRoll.doUpdate()
self.wrist.doUpdate()
self.handDOF1.doUpdate()
def doVelocityUpdate(self):
if self.shoulderYawCocont < 0:
self.shoulderYaw.moveWith(self.shoulderYawVel,
abs(self.shoulderYawCocont))
else:
self.shoulderYaw.servoWith(self.shoulderYawVel,
self.shoulderYawCocont)
if self.shoulderRollCocont < 0:
self.shoulderRoll.moveWith(self.shoulderRollVel,
abs(self.shoulderRollCocont))
else:
self.shoulderRoll.servoWith(self.shoulderRollVel,
self.shoulderRollCocont)
if self.shoulderPitchCocont < 0:
self.shoulderPitch.moveWith(self.shoulderPitchVel,
abs(self.shoulderPitchCocont))
else:
self.shoulderPitch.servoWith(self.shoulderPitchVel,
self.shoulderPitchCocont)
self.upperarmRoll.servoWith(self.upperarmRollVel)
if self.elbowCocont < 0:
self.elbow.moveWith(self.elbowVel, abs(self.elbowCocont))
else:
self.elbow.servoWith(self.elbowVel, self.elbowCocont)
self.forearmRoll.servoWith(self.forearmRollVel)
if self.wristCocont == -999:
self.wrist.passiveHold(0.0)
else:
if self.wristCocont < 0:
self.wrist.moveWith(self.wristVel, abs(self.wristCocont))
else:
self.wrist.servoWith(self.wristVel, self.wristCocont)
self.handDOF1.servoWith(self.handDOF1Vel)
self.publishJointState()
def publishJointState(self):
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = self.jointNames
msg.position = self.getJointAngles()
self.jointStatePub.publish(msg)
def getJointAngles(self):
angles = list()
angles.append(self.shoulderYaw.getJointAngle())
angles.append(self.shoulderRoll.getJointAngle())
angles.append(self.shoulderPitch.getJointAngle())
angles.append(self.upperarmRoll.getJointAngle())
angles.append(self.elbow.getJointAngle())
angles.append(self.forearmRoll.getJointAngle())
angles.append(self.wrist.getJointAngle())
angles.append(self.handDOF1.getJointAngle())
return angles
def setVelocity(self, velocities):
self.shoulderYawVel = velocities[0]
self.shoulderRollVel = velocities[1]
self.shoulderPitchVel = velocities[2]
self.upperarmRollVel = velocities[3]
self.elbowVel = velocities[4]
self.forearmRollVel = velocities[5]
self.wristVel = velocities[6]
self.handDOF1Vel = velocities[7]
def setCocontraction(self, shoulderYaw, shoulderRoll, shoulderPitch, elbow,
wrist):
self.shoulderYawCocont = shoulderYaw
self.shoulderRollCocont = shoulderRoll
self.shoulderPitchCocont = shoulderPitch
self.elbowCocont = elbow
self.wristCocont = wrist
def servoTo(self, positions):
if self.teleop == 0:
self.shoulderYaw.servoTo(positions[0], self.shoulderYawCocont)
self.shoulderRoll.servoTo(positions[1], self.shoulderRollCocont)
self.shoulderPitch.servoTo(positions[2], self.shoulderPitchCocont)
self.upperarmRoll.servoTo(positions[3])
self.elbow.servoTo(positions[4], self.elbowCocont)
self.forearmRoll.servoTo(positions[5])
self.wrist.servoTo(positions[6], self.wristCocont)
self.handDOF1.servoTo(positions[7])
self.publishJointState()
else:
print(
"WARNING: Asked to servo to pose, but ignoring as in teleop mode. Check gummi.yaml file."
)
def goTo(self, positions, now):
if self.teleop == 0:
self.shoulderYaw.goTo(positions[0], self.shoulderYawCocont, now)
self.shoulderRoll.goTo(positions[1], self.shoulderRollCocont, now)
self.shoulderPitch.goTo(positions[2], self.shoulderPitchCocont,
now)
self.upperarmRoll.servoTo(positions[3])
self.elbow.goTo(positions[4], self.elbowCocont, now)
self.forearmRoll.servoTo(positions[5])
self.wrist.goTo(positions[6], self.wristCocont, now)
self.handDOF1.servoTo(positions[7])
self.publishJointState()
else:
print(
"WARNING: Asked to go to pose, but ignoring as in teleop mode. Check gummi.yaml file."
)
def goRestingPose(self, now):
self.shoulderYaw.goTo(0, self.shoulderYawCocont, now)
self.shoulderRoll.goTo(0, self.shoulderRollCocont, now)
self.shoulderPitch.goTo(0, self.shoulderPitchCocont, now)
self.upperarmRoll.servoTo(0)
self.elbow.goTo(0, self.elbowCocont, now)
self.forearmRoll.servoTo(0)
self.wrist.goTo(0, self.wristCocont, now)
self.handDOF1.servoTo(0)
def passiveHold(self):
self.shoulderYaw.passiveHold(self.shoulderYawCocont)
self.shoulderRoll.passiveHold(self.shoulderRollCocont)
self.shoulderPitch.passiveHold(self.shoulderPitchCocont)
self.upperarmRoll.doUpdate()
self.elbow.passiveHold(self.elbowCocont)
self.forearmRoll.doUpdate()
self.wrist.passiveHold(self.wristCocont)
self.handDOF1.doUpdate()
def doGradualStartup(self):
self.shoulderYaw.moveTo(-0.05, self.shoulderYawCocont)
rospy.sleep(1)
self.shoulderRoll.moveTo(-0.25, self.shoulderRollCocont)
rospy.sleep(1)
self.shoulderPitch.moveTo(0.25, self.shoulderPitchCocont)
rospy.sleep(1)
self.upperarmRoll.servoTo(0)
rospy.sleep(1)
self.elbow.moveTo(-0.6, self.elbowCocont)
rospy.sleep(1)
self.forearmRoll.servoTo(0)
rospy.sleep(1)
self.wrist.moveTo(0, self.wristCocont)
rospy.sleep(1)
self.handDOF1.servoTo(-2.2)
rospy.sleep(1)
self.headYaw.servoTo(0)
rospy.sleep(1)
def setCollisionResponses(self, shoulderYaw, shoulderRoll, shoulderPitch,
elbow, wrist):
self.shoulderYaw.setCollisionResponse(shoulderYaw)
self.shoulderRoll.setCollisionResponse(shoulderRoll)
self.shoulderPitch.setCollisionResponse(shoulderPitch)
self.elbow.setCollisionResponse(elbow)
self.wrist.setCollisionResponse(wrist)
def doZeroEquilibriumPose(self):
self.shoulderYaw.moveTo(0.0, self.shoulderYawCocont)
rospy.sleep(1)
self.shoulderRoll.moveTo(0.0, self.shoulderRollCocont)
rospy.sleep(1)
self.shoulderPitch.moveTo(0.0, self.shoulderPitchCocont)
rospy.sleep(1)
self.upperarmRoll.servoTo(0)
rospy.sleep(1)
self.elbow.moveTo(0.0, self.elbowCocont)
rospy.sleep(1)
self.forearmRoll.servoTo(0)
rospy.sleep(1)
self.wrist.moveTo(0, self.wristCocont)
rospy.sleep(1)
self.handDOF1.servoTo(0.0)
rospy.sleep(1)
self.headYaw.servoTo(0)
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()