class DirectDrive: def __init__(self, name, servoRange): self.name = name self.servoRange = servoRange self.initPublishers() self.initVariables() self.angle = JointAngle(name, 1, -servoRange / 2, servoRange / 2, False) def initPublishers(self): self.pub = rospy.Publisher(self.name + '_controller/command', Float64, queue_size=5) def initVariables(self): self.velocity = False self.noCommandYet = True def servoTo(self, dAngle): self.velocity = False self.angle.setDesired(dAngle) self.noCommandYet = False self.doUpdate() def servoWith(self, dVelocity): self.velocity = True self.angle.setDesiredVelocity(dVelocity) self.noCommandYet = False self.doUpdate() def publishCommand(self): dAngle = self.angle.getDesired() self.pub.publish(dAngle) def doUpdate(self): if self.velocity: self.angle.doVelocityIncrement() if self.noCommandYet: self.angle.setDesired(self.encoderAngle) self.publishCommand() def getJointAngle(self): return self.angle.getEncoder() def setTorqueLimit(self, limit): service_name = self.name + "_controller/set_torque_limit" rospy.wait_for_service(service_name) try: te = rospy.ServiceProxy(service_name, SetTorqueLimit) te(limit) except rospy.ServiceException, e: print "Service call failed: %s" % e
class DirectDrive: def __init__(self, name, servoRange): self.name = name self.servoRange = servoRange self.initPublishers() self.initVariables() self.angle = JointAngle(name, 1, -servoRange / 2, servoRange / 2, False) self.recording = Recording() def initPublishers(self): self.pub = rospy.Publisher(self.name + "_controller/command", Float64, queue_size=5) def initVariables(self): self.velocity = False self.noCommandYet = True def servoTo(self, dAngle): self.velocity = False self.angle.setDesired(dAngle) self.noCommandYet = False self.doUpdate() def servoWith(self, dVelocity): self.velocity = True self.angle.setDesiredVelocity(dVelocity) self.noCommandYet = False self.doUpdate() def publishCommand(self): dAngle = self.angle.getDesired() self.pub.publish(dAngle) def doUpdate(self): if self.velocity: self.angle.doVelocityIncrement() if self.noCommandYet: self.angle.setDesired(self.encoderAngle) self.publishCommand() def getJointAngle(self): return self.angle.getEncoder() def prepareRecording(self, fileNameBase): fileName = fileNameBase + "_" + self.name + ".csv" self.recording.prepare(fileName, ["time", "angle"]) def recordLine(self, delta): angle = self.getJointAngle() self.recording.add([delta, angle])