Commits

Trammell Hudson  committed 8a3ce3e

use last command, not last fk position

  • Participants
  • Parent commits edeff5a

Comments (0)

Files changed (1)

 		self.live = True
 		self.joints = joints
 		self.counts = [0,0,0,0,0]
+		self.last_xyz = False
 		self.fk = PumaFK(joints)
 		self.ik = PumaIK(False, True, joints)
 		self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
 
 	# Move linearly to an IK computed position at mm/s speed
 	# This will hold the current tool configuration
+	# The current xyz position is the last xyz position sent
+	# or if we have had an intermediate save command, the last
+	# computed FK position.  Otherwise we have a "step" when
+	# the new xyz does not match the current xyz.
 	def move_to(self, vel, dst_xyz):
-		xyz = self.fk.p[:]
+		if self.last_xyz:
+			xyz = self.last_xyz[:]
+		else:
+			xyz = self.fk.p[:]
+
 		cur_a = self.fk.a[:]
 		cur_s = self.fk.s[:]
 		cur_n = self.fk.n[:]
 
+		# Use a tool-down, face forward ASN
+		cur_a = [0,0,-1]
+		cur_s = [0,1,0]
+		cur_n = [-1,0,0]
+
 		dx = dst_xyz[0] - xyz[0]
 		dy = dst_xyz[1] - xyz[1]
 		dz = dst_xyz[2] - xyz[2]
 			xyz[1] += dy
 			xyz[2] += dz
 			self.move_ik(dt*1000, xyz, cur_a, cur_s, cur_n)
-			time.sleep(dt * 0.6)
+			time.sleep(dt * 0.4)
+		self.last_xyz = xyz[:]
 
 
 if __name__ == "__main__":
 			continue
 		if line == "hold":
 			r.move_raw(1000, r.counts)
+			r.last_xyz = False
+			print "Holding " + str(r.counts) + ": xyz " + str([int(x) for x in r.fk.p])
 			continue
 		if line == "r":
 			print r.counts