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)
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])
def __init__(self, name, servoRange=None): self.name = name self.initPublishers() self.initVariables() if servoRange: minRange = -servoRange / 2 maxRange = servoRange / 2 else: minRange = rospy.get_param("~" + name + "/minAngle") maxRange = rospy.get_param("~" + name + "/maxAngle") self.angle = JointAngle(name, 1, minRange, maxRange, False)
def __init__(self, name): self.name = name self.signEquilibrium = fetchParam("~" + self.name + "/signEquilibrium", 1) self.signFlexor = rospy.get_param("~" + self.name + "/signFlexor") self.signExtensor = rospy.get_param("~" + self.name + "/signExtensor") self.signEncoder = rospy.get_param("~" + self.name + "/signEncoder") self.signJoint = rospy.get_param("~" + self.name + "/signJoint") self.nameFlexor = rospy.get_param("~" + self.name + "/nameFlexor") self.nameExtensor = rospy.get_param("~" + self.name + "/nameExtensor") self.nameEncoder = rospy.get_param("~" + self.name + "/nameEncoder") self.servoRange = rospy.get_param("~" + self.name + "/servoRange") self.servoOffset = rospy.get_param("~" + self.name + "/servoOffset") self.minAngle = rospy.get_param("~" + self.name + "/minAngle") self.maxAngle = rospy.get_param("~" + self.name + "/maxAngle") self.angleOffset = rospy.get_param("~" + self.name + "/angleOffset") self.pGain = rospy.get_param("~" + self.name + "/gains/P") self.vGain = rospy.get_param("~" + self.name + "/gains/D") self.angle = JointAngle(self.nameEncoder, self.signEncoder, self.minAngle, self.maxAngle, True) self.flexorAngle = JointAngle(self.nameFlexor, self.signFlexor, -1000, 1000, False) self.extensorAngle = JointAngle(self.nameExtensor, self.signExtensor, -1000, 1000, False) self.stretchReflex = Reflex(0, 0.02, 0.01) self.compliance = Reflex(15, 0.02, 0.01) self.recording = Recording() self.initPublishers() self.initVariables() self.disableEncoderTorque() self.setTorqueLimit(self.nameExtensor, 1) self.setTorqueLimit(self.nameFlexor, 1) self.calculateEqVelCalibration()
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 __init__(self, name): self.name = name self.calibrated = rospy.get_param("~" + self.name + "/calibrated") self.signEncoder = rospy.get_param("~" + self.name + "/signEncoder") self.signJoint = rospy.get_param("~" + self.name + "/signJoint") self.name = rospy.get_param("~" + self.name + "/name") self.nameEncoder = rospy.get_param("~" + self.name + "/nameEncoder") minAngle = rospy.get_param("~" + self.name + "/minAngle") maxAngle = rospy.get_param("~" + self.name + "/maxAngle") self.pGain = rospy.get_param("~" + self.name + "/gains/P") self.iGain = rospy.get_param("~" + self.name + "/gains/I") self.vGain = rospy.get_param("~" + self.name + "/gains/D") self.maxAbsForwardError = rospy.get_param("~" + self.name + "/maxAbsForwardError") # Attribute stores resting pose angle of antagonist-pair motor joints # taken from joint yaml file in gummi_base and gummi_ee pkgs. self.restingPoseAngle = rospy.get_param("~" + self.name + "/restingPoseAngle") self.range = maxAngle - minAngle self.angle = JointAngle(self.nameEncoder, self.signEncoder, minAngle, maxAngle, True) self.eqModel = EquilibriumModel(self.name) self.inverseModel = InverseModel(self.name) self.inverseModelCollision = InverseModel(self.name) self.forwardModel = ForwardModel(self.name) if self.calibrated is 1: self.inverseModel.loadCalibration() self.inverseModelCollision.loadCalibration() self.forwardModel.loadCalibration() self.cocontractionReflex = Reflex(2.0, 0.0045, 0.0) self.feedbackReflex = Reflex(1.0, 0.0075, 0.0) self.collisionReflex = Reflex(1.0, 0.0075, 0.0) self.initPublishers() self.initVariables() self.disableEncoderTorque() jointRange = self.angle.getMax() - self.angle.getMin() self.eqModel.calculateEqVelCalibration(jointRange)
def __init__(self, name): self.name = name self.calibrated = rospy.get_param("~" + self.name + "/calibrated") self.signEncoder = rospy.get_param("~" + self.name + "/signEncoder") self.signJoint = rospy.get_param("~" + self.name + "/signJoint") self.name = rospy.get_param("~" + self.name + "/name") self.nameEncoder = rospy.get_param("~" + self.name + "/nameEncoder") minAngle = rospy.get_param("~" + self.name + "/minAngle") maxAngle = rospy.get_param("~" + self.name + "/maxAngle") self.pGain = rospy.get_param("~" + self.name + "/gains/P") self.iGain = rospy.get_param("~" + self.name + "/gains/I") self.vGain = rospy.get_param("~" + self.name + "/gains/D") self.maxAbsForwardError = rospy.get_param("~" + self.name + "/maxAbsForwardError") self.range = maxAngle - minAngle self.angle = JointAngle(self.nameEncoder, self.signEncoder, minAngle, maxAngle, True) self.eqModel = EquilibriumModel(self.name) self.inverseModel = InverseModel(self.name) self.inverseModelCollision = InverseModel(self.name) self.forwardModel = ForwardModel(self.name) if self.calibrated is 1: self.inverseModel.loadCalibration() self.inverseModelCollision.loadCalibration() self.forwardModel.loadCalibration() self.cocontractionReflex = Reflex(2.0, 0.0015, 0.0) self.feedbackReflex = Reflex(1.0, 0.0075, 0.0) self.collisionReflex = Reflex(1.0, 0.0075, 0.0) self.initPublishers() self.initVariables() self.disableEncoderTorque() jointRange = self.angle.getMax() - self.angle.getMin() self.eqModel.calculateEqVelCalibration(jointRange)
class Antagonist: def __init__(self, name): self.name = name self.calibrated = rospy.get_param("~" + self.name + "/calibrated") self.signEncoder = rospy.get_param("~" + self.name + "/signEncoder") self.signJoint = rospy.get_param("~" + self.name + "/signJoint") self.name = rospy.get_param("~" + self.name + "/name") self.nameEncoder = rospy.get_param("~" + self.name + "/nameEncoder") minAngle = rospy.get_param("~" + self.name + "/minAngle") maxAngle = rospy.get_param("~" + self.name + "/maxAngle") self.pGain = rospy.get_param("~" + self.name + "/gains/P") self.iGain = rospy.get_param("~" + self.name + "/gains/I") self.vGain = rospy.get_param("~" + self.name + "/gains/D") self.maxAbsForwardError = rospy.get_param("~" + self.name + "/maxAbsForwardError") self.range = maxAngle - minAngle self.angle = JointAngle(self.nameEncoder, self.signEncoder, minAngle, maxAngle, True) self.eqModel = EquilibriumModel(self.name) self.inverseModel = InverseModel(self.name) self.inverseModelCollision = InverseModel(self.name) self.forwardModel = ForwardModel(self.name) if self.calibrated is 1: self.inverseModel.loadCalibration() self.inverseModelCollision.loadCalibration() self.forwardModel.loadCalibration() self.cocontractionReflex = Reflex(2.0, 0.0015, 0.0) self.feedbackReflex = Reflex(1.0, 0.0075, 0.0) self.collisionReflex = Reflex(1.0, 0.0075, 0.0) self.initPublishers() self.initVariables() self.disableEncoderTorque() jointRange = self.angle.getMax() - self.angle.getMin() self.eqModel.calculateEqVelCalibration(jointRange) def initVariables(self): self.errors = deque() self.velocity = False self.closedLoop = False self.feedForward = False self.collisionResponse = False self.errorLast = 0.0 self.ballistic = 0.0 self.deltaAngleBallistic = 0.0 self.deltaEqFeedback = 0.0 self.lastForwardError = 0.0 self.forwardError = 0.0 self.ballisticRatio = 0.85 self.feedbackRatio = 0.5 def disableEncoderTorque(self): service_name = self.nameEncoder + "_controller/torque_enable" rospy.wait_for_service(service_name) try: te = rospy.ServiceProxy(service_name, TorqueEnable) te(torque_enable=False) except rospy.ServiceException, e: print "Service call failed: %s" % e
class Antagonist: def __init__(self, name): self.name = name self.signEquilibrium = fetchParam("~" + self.name + "/signEquilibrium", 1) self.signFlexor = rospy.get_param("~" + self.name + "/signFlexor") self.signExtensor = rospy.get_param("~" + self.name + "/signExtensor") self.signEncoder = rospy.get_param("~" + self.name + "/signEncoder") self.signJoint = rospy.get_param("~" + self.name + "/signJoint") self.nameFlexor = rospy.get_param("~" + self.name + "/nameFlexor") self.nameExtensor = rospy.get_param("~" + self.name + "/nameExtensor") self.nameEncoder = rospy.get_param("~" + self.name + "/nameEncoder") self.servoRange = rospy.get_param("~" + self.name + "/servoRange") self.servoOffset = rospy.get_param("~" + self.name + "/servoOffset") self.minAngle = rospy.get_param("~" + self.name + "/minAngle") self.maxAngle = rospy.get_param("~" + self.name + "/maxAngle") self.angleOffset = rospy.get_param("~" + self.name + "/angleOffset") self.pGain = rospy.get_param("~" + self.name + "/gains/P") self.vGain = rospy.get_param("~" + self.name + "/gains/D") self.angle = JointAngle(self.nameEncoder, self.signEncoder, self.minAngle, self.maxAngle, True) self.flexorAngle = JointAngle(self.nameFlexor, self.signFlexor, -1000, 1000, False) self.extensorAngle = JointAngle(self.nameExtensor, self.signExtensor, -1000, 1000, False) self.stretchReflex = Reflex(0, 0.02, 0.01) self.compliance = Reflex(15, 0.02, 0.01) self.recording = Recording() self.initPublishers() self.initVariables() self.disableEncoderTorque() self.setTorqueLimit(self.nameExtensor, 1) self.setTorqueLimit(self.nameFlexor, 1) self.calculateEqVelCalibration() def initVariables(self): self.commandFlexor = 0 self.commandExtensor = 0 self.dEquilibrium = 0 self.pGainUse = 0 self.vGainUse = 0 self.lGain = 0.03 self.dStiffness = 0 self.cStiffness = 0 self.velocity = False self.closedLoop = False self.maxStiffness = 1 self.meanError = 0 self.maxLoad = 10000 self.loadRatio = 0 self.errorLast = 0.0 self.dEqVelCalibration = 1.0 self.equilibriumErrors = list() for i in range(0, 5): self.equilibriumErrors.append(0.0) def calculateEqVelCalibration(self): joint_range = self.angle.getMax() - self.angle.getMin() eq_range = 2 * 2.0 self.dEqVelCalibration = eq_range/joint_range; print("Equilibrium to joint velocity calibration: " + str(self.dEqVelCalibration) + ".") def setTorqueLimit(self, name, limit): service_name = 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