Completed robot

scripts/fruitbot.py

@@ -21,6 +21,41 @@ from sensor_msgs.msg   import JointState
 from hw6code.kinematics import Kinematics # todo: can't import from this package
                                           # due to hyphen in name
 
+TWO_PI = 2 * np.pi
+
+# finds number equal to q1 mod 2pi
+# which is closest to q0
+def closest_rdr_mod_2pi(q0, q1):
+    q0 /= TWO_PI
+    q1 /= TWO_PI
+    dif = q1 - q0
+    lo = q1 - np.ceil(dif)
+    hi = q1 - np.floor(dif)
+    # print(f'q0 = {q0}, q1 = {q1}, lo = {lo}, hi = {hi}')
+    # assert np.allclose(q1 - lo, np.rint(q1 - lo))
+    # assert np.allclose(q1 - hi, np.rint(q1 - hi))
+    # assert (q0 - lo) <= 1.001
+    # assert (q0 - hi) <= 1.001
+    # assert np.allclose(abs(hi - lo), 1)
+    return TWO_PI * lo if abs(q0 - lo) < abs(q0 - hi) else TWO_PI * hi
+
+def mod_pi_range(x):
+    return ((x + np.pi) % TWO_PI) - np.pi
+
+def interp_away_from_pi(interp_factor, start, end):
+    start_0 = mod_pi_range(start)
+    end_0 = mod_pi_range(end)
+    return (1 - interp_factor) * start_0 + interp_factor * end_0
+
+def interp_joints(q0, q1, i):
+    return np.array([
+        (1 - i) * q0[0] + i * q1[0],
+        (1 - i) * q0[1] + i * q1[1],
+        interp_away_from_pi(i, q0[2], q1[2]),
+        (1 - i) * q0[3] + i * q1[3],
+        (1 - i) * q0[4] + i * q1[4],
+        (1 - i) * q0[5] + i * q1[5],
+    ])
 
 def totalError(kin, pd, Rd, q):
     p = np.zeros((3,1)) # posn
@@ -39,13 +74,16 @@ def totalError(kin, pd, Rd, q):
     res = np.hstack((ep, er))
     return res
 
-def ikin(kin, pd, Rd, J, q, clamp, iterations=10):
+def ikin(kin, pd, Rd, J, q, iterations=25):
+    q_init = q.copy()
     for _ in range(iterations):
         e = totalError(kin, pd, Rd, q)
         kin.Jac(q, J)
         pinv = J.T.dot(np.linalg.inv(J.dot(J.T)))
-        delta = pinv.dot(e).clip(-clamp, clamp)
+        delta = pinv.dot(e)
         q += delta
+    for i in range(q.shape[0]):
+        q[i] = closest_rdr_mod_2pi(q_init[i], q[i])
 
 #
 #  Basic Marker
@@ -64,6 +102,13 @@ def BasicMarker():
     marker.color.a            = 1.0     # Make non-transparent!
     return marker
 
+def spline(t0, t1, t):
+    if t <= t0: return 0
+    if t >= t1: return 1
+    x = (t - t0) / (t1 - t0)
+    return x * x * (3 - 2 * x)
+
+
 #
 #  Add a Cube
 #
@@ -107,7 +152,26 @@ fruit_ids = []
 GRAV_ACCEL = -0.5
 WORKSPACE_CENTER = np.array([0., 0., 0.6])
 WORKSPACE_RADIUS_MIN = 0.2
-WORKSPACE_RADIUS_MAX = 0.6
+WORKSPACE_RADIUS_MAX = 0.7
+INITIAL_MOVE_TIME = 1
+SLICE_BACKWARD = -1.5
+SLICE_FORWARD = 0.7
+SLICE_TIME = 0.2
+
+
+# stb
+# should satisfy
+# interp(-stb, SLICE_TIME - stb, 0)
+
+SLICE_TIME_BEFORE = 0
+SLICE_TIME_AFTER = 0
+
+for s in np.arange(0, SLICE_TIME, 0.00001):
+    SLICE_TIME_BEFORE = s
+    SLICE_TIME_AFTER = SLICE_TIME - s
+    i = spline(-SLICE_TIME_BEFORE, SLICE_TIME_AFTER, 0)
+    if (1 - i) * SLICE_BACKWARD + i * SLICE_FORWARD >= 0:
+        break
 
 class Fruit:
     def __init__(self, ipos, ivel, tzero):
@@ -138,6 +202,11 @@ class Fruit:
         self.marker.color.b         = color[2]
         self.moveMarker(0)
 
+    def hit(self):
+        self.marker.color.r = 1
+        self.marker.color.g = 0
+        self.marker.color.b = 0
+
     def pos(self, t):
         ts = t - self.tzero
         return np.array([
@@ -170,6 +239,9 @@ class Fruit:
     def cleanUp(self):
         fruit_ids.remove(self.marker.id)
 
+SETTING_UP = 0
+SLICING = 1
+
 #
 #  Main Code
 #
@@ -189,15 +261,6 @@ if __name__ == "__main__":
     N   = kin.dofs()
     rospy.loginfo("Loaded URDF for %d joints" % N)
 
-    # Wait for '/rviz' to subscribe.  Turns out ROS may add a unique
-    # number to the rviz name.  And in general, the notion of topics
-    # really doesn't lend itself to knowing who is listening.  So
-    # let's just skip...
-    #rospy.loginfo("Waiting for rviz to subscribe...")
-    #while not rospy.is_shutdown() and not pub.impl.has_connection('/rviz'):
-    #    print(pub.get_num_connections(), end="..", flush=True)
-    #    rospy.sleep(0.1)
-    #print()
 
     t = 0
     rate  = 100;
@@ -210,11 +273,24 @@ if __name__ == "__main__":
 
 
     goal_pos = np.array([0.3, 0.3, 0.6])
-    goal_rot = np.identity(3) 
+    goal_rot = np.identity(3)
+
+    setup_init = np.random.uniform(-1, 1, kin.dofs())
+    setup_final = np.random.uniform(-1, 1, kin.dofs())
+    setup_t0 = 0
+    setup_t1 = 1
+    col_time = 0
+
+    done_slicing = True
+
+    slice_final = np.random.uniform(-1, 1, kin.dofs())
+    currently_slicing = None
 
     cubes = [ ]
+    
     while not rospy.is_shutdown():
-        if len(cubes) == 0:
+        if done_slicing:
+            
             new_fruit = Fruit(
                 np.array([-3., 0., 0.]),
                 np.array([
@@ -231,7 +307,7 @@ if __name__ == "__main__":
             collision_times = []
             while True:
                 posn = new_fruit.pos(tp)
-                if posn[2] < -10:
+                if posn[2] < -1:
                     break
                 if WORKSPACE_RADIUS_MIN <= np.linalg.norm(WORKSPACE_CENTER - posn) <= WORKSPACE_RADIUS_MAX:
                     collision_times.append(tp)
@@ -242,6 +318,7 @@ if __name__ == "__main__":
                 col_time = collision_times[0]
                 col_pos  = new_fruit.pos(col_time)
                 col_vel  = new_fruit.vel(col_time)
+                col_unit_vel = col_vel / np.linalg.norm(col_vel)
 
                 col_rot  = np.identity(3)
 
@@ -252,24 +329,34 @@ if __name__ == "__main__":
                 assert np.allclose(0, np.dot(x_rot, z_rot))
                 y_rot = np.cross(z_rot, x_rot)
 
-                goal_pos = col_pos
-                goal_rot = np.vstack((x_rot, y_rot, z_rot)).T
-
                 print(f'Selected a colision at {col_time}')
                 print(f'Position: {col_pos}')
                 print(f'Velocity: {col_vel}')
-            else:
-                print(f'Probably cannot slice this fruit')
+                setup_t0 = t
+                setup_t1 = min(t + INITIAL_MOVE_TIME, col_time - SLICE_TIME_BEFORE)
+                setup_init = slice_final.copy()
+                ikin(kin, col_pos, np.vstack((x_rot, y_rot, z_rot)).T, J, setup_final, 500)
 
+                
 
 
+                slice_final = setup_final.copy()
+                slice_final[-1] += SLICE_FORWARD - SLICE_BACKWARD
+
+                # print(f'setup_init = {setup_init}')
+                # print(f'setup_final = {setup_final}')
+
+                done_slicing = False
+                currently_slicing = new_fruit
+            else:
+                print(f'Probably cannot slice this fruit')
 
         t += dt
 
         new_cubes = []
         for cube in cubes:
             cube.moveMarker(t)
-            if cube.pos(t)[2] > -5:
+            if cube.pos(t)[2] > -1000:
                 new_cubes.append(cube)
             else:
                 cube.cleanUp()
@@ -285,20 +372,32 @@ if __name__ == "__main__":
 
         viz_pub.publish(ar)
 
-        # inverse kinematics
-
-        ikin(kin, goal_pos, goal_rot, J, q, 1000)
-
         msg = JointState()
-        msg.name.append('theta1')    # Keep appending joint names
+        msg.name.append('theta1')
         msg.name.append('theta2')
         msg.name.append('theta3')
         msg.name.append('theta4')
         msg.name.append('theta5')
         msg.name.append('theta6')
 
-        msg.position = q
+        # if t >= setup_t1:
+        #     mode = SETTING_UP
+        # elif t <= slice_end_t1
+
+        interp = spline(setup_t0, setup_t1, t)
+        joints = interp_joints(setup_init, setup_final, interp)
+
+        swing_interp = spline(col_time - SLICE_TIME_BEFORE, col_time + SLICE_TIME_AFTER, t)
+        joints[-1] += (1 - swing_interp) * SLICE_BACKWARD + swing_interp * SLICE_FORWARD
+        msg.position = joints
+        
         msg.header.stamp = rospy.Time.now()
+
+        if t >= col_time and currently_slicing:
+            currently_slicing.hit()
+        if t >= col_time + SLICE_TIME_AFTER + 0.5:
+            done_slicing = True
+
         joint_pub.publish(msg)
 
         servo.sleep()

urdf/samurai.urdf

@@ -71,7 +71,7 @@
   <joint name="holder" type="fixed">
     <parent link="link6"/>
     <child  link="katana"/>
-    <origin xyz="0 0.1 0" rpy="0 0 0"/>
+    <origin xyz="0 0.1 0" rpy="-1.57079632679489661923 1.57079632679489661923 0"/>
   </joint>
 
   <link name="katana">