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
import numpy as np
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from push_env import PushingEnv
from forward_model import ForwardModel
from inverse_model import InverseModel
if __name__ == "__main__":
# train inverse model
inverse_model = InverseModel()
num_epochs = 30
train_losses, valid_losses = inverse_model.train(num_epochs=num_epochs)
#train forward model
forward_model = ForwardModel()
num_epochs = 30
train_losses, valid_losses = forward_model.train(num_epochs=num_epochs)
env = PushingEnv(ifRender=False)
num_trials = 10
# two pushes, inverse model
errors = np.zeros(num_trials)
# save one push
errors[0] = env.plan_inverse_model_extrapolate(
inverse_model, img_save_name="inverse_twopush", seed=0)
print("test loss:", errors[0])
# try 10 random seeds