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
