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)
