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
