diff --git a/scripts/fruitbot.py b/scripts/fruitbot.py
index a3f95d8341f96ce7ed76bad7b90a0d20358d4756..1d4ab60263e15d81a0dfe258934b4c91dd6173e5 100755
--- a/scripts/fruitbot.py
+++ b/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()
diff --git a/urdf/samurai.urdf b/urdf/samurai.urdf
index 893884aca073e2d30c78c643b0261333939aafe5..30efca095c544e79e6c6a33005330e6b9ad35469 100644
--- a/urdf/samurai.urdf
+++ b/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">