# HG changeset patch # User Trammell Hudson # Date 1378324182 14400 # Node ID febeae045bf5594bc2c8479c649e7966dc7c77f1 # Parent 3bb4a204be76de1a1ecd68ac8d483e21196a115c Add raw angle command to test angle calibrations diff --git a/Puma.py b/Puma.py --- a/Puma.py +++ b/Puma.py @@ -232,6 +232,13 @@ if line == "a": print [round(x * 180 / pi, 1) for x in r.fk.theta] continue + g = re.match("^a (-?[0-9]+) (-?[0-9]+) (-?[0-9]+) (-?[0-9]+) (-?[0-9]+) (-?[0-9]+)", line) + if g is not None: + theta = [int(x) * pi / 180 for x in g.group(1,2,3,4,5,6)] + counts = r.ik.angle2count(theta, []) + print counts + r.move_raw(10000, counts) + continue if line == "p": print [int(x) for x in r.fk.p], [round(x,2) for x in r.fk.a], [round(x,2) for x in r.fk.s], [round(x,2) for x in r.fk.n] continue diff --git a/PumaIK.py b/PumaIK.py --- a/PumaIK.py +++ b/PumaIK.py @@ -141,12 +141,15 @@ return True def get_counts(self, current_theta): + return self.angle2count(self.theta, current_theta) + + def angle2count(self, dest_theta, current_theta): counts = [] for i in range(0, 6): # Adjust by the home position offset to the # joint-frame angle. j = self.joints[i] - a = self.theta[i] + a = dest_theta[i] # Elbow joint is off by 90 deg if i == 2 and a < 0: