def compute_twist(rbt):
"""
Computes the corresponding twist for the rigid body transform
Input:
rbt - (4,4) ndarray
Output:
v - (3,) array
w - (3,) array
"""
#YOUR CODE HERE
rot_matrix = rbt[0:3, 0:3]
trans = rbt[0:3, 3]
(omega, theta) = eqf.find_omega_theta(rot_matrix)
v = eqf.find_v(omega, theta, trans)
v = v.flatten()
#v = np.array(3,1)
#w = np.array(3,1)
#print(v)
#print(omega)
return (v, omega)
def compute_twist(rbt):
"""
Computes the corresponding twist for the rigid body transform
Input:
rbt - (4,4) ndarray
Output:
v - (3,) array
w - (3,) array
"""
R = rbt[0:3][:, 0:3]
trans = rbt[0:3][:, 3:4]
[omega, theta] = eqf.find_omega_theta(R)
v = eqf.find_v(omega, theta, trans)
#pdb.set_trace()
v = np.array([v[0, 0], v[1, 0], v[2, 0]])
twist = np.array([v[0], v[1], v[2], omega[0], omega[1], omega[2]])
return twist
def compute_twist(rbt):
"""
Computes the corresponding twist for the rigid body transform
Input:
rbt - (4,4) ndarray
Output:
v - (3,) array
w - (3,) array
"""
#YOUR CODE HERE
rot_matrix = rbt[0:3,0:3]
trans = rbt[0:3,3]
(omega,theta) = eqf.find_omega_theta(rot_matrix)
v = eqf.find_v(omega,theta,trans)
v = v.flatten()
#v = np.array(3,1)
#w = np.array(3,1)
#print(v)
#print(omega)
return (v,omega)
def getMsgFromTransform(trans, rot):
omega, theta = eqf.quaternion_to_exp(np.array(rot))
omega = np.reshape(omega, (3, 1))
v = eqf.find_v(omega, theta, trans)
twist = np.vstack((v, omega))
linear = Vector3()
computeScale = lambda x, y, z, target: (float(target) /
(x * x + y * y + z * z))**(0.5)
a = computeScale(v[0], v[1], v[2], speed)
print "Scale", a
linear.x = v[0] * a
linear.y = v[1] * a
linear.z = v[2] * a
angular = Vector3()
angular.x = omega[0] * theta / 2
angular.y = omega[1] * theta / 2
angular.z = omega[2] * theta / 2
message = Twist()
message.linear = linear
message.angular = angular
return message
def compute_twist(rbt):
"""
Computes the corresponding twist for the rigid body transform
Input:
rbt - (4,4) ndarray
Output:
v - (3,) array
w - (3,) array
"""
#YOUR CODE HERE
omega, theta = eqf.find_omega_theta(rbt)
v = eqf.find_v(omega, theta, rbt[:3, 3])
return (v,omega)
def compute_twist(rbt):
"""
Computes the corresponding twist for the rigid body transform
Input:
rbt - (4,4) ndarray
Output:
v - (3,) array
w - (3,) array
"""
#YOUR CODE HERE
R = rbt[:3, :3]
trans = rbt[:3, 3]
orientation = eqf.find_omega_theta(R) # omega/theta
v = eqf.find_v(orientation[0], orientation[1], trans).reshape(3, )
return (v, orientation[0])
def compute_twist(rbt):
"""
Computes the corresponding twist for the rigid body transform
Input:
rbt - (4,4) ndarray
Output:
v - (3,) array
w - (3,) array
"""
r = rbt[:3,:3]
omega, theta = eqf.find_omega_theta(r)
trans = rbt[0:3,3]
v = eqf.find_v(omega, theta, trans)
w = omega
return (v,w)
def getMsgFromTransform(trans, rot):
omega, theta = eqf.quaternion_to_exp(np.array(rot))
omega = np.reshape(omega, (3, 1))
v = eqf.find_v(omega, theta, trans)
twist = np.vstack((v, omega))
linear = Vector3()
linear.x = v[0] * speed
linear.y = v[1] * speed
linear.z = v[2] * speed
angular = Vector3()
angular.x = omega[0] * theta / 2
angular.y = omega[1] * theta / 2
angular.z = omega[2] * theta / 2
message = Twist()
message.linear = linear
message.angular = angular
return message
def compute_twist(rbt):
"""
Computes the corresponding twist for the rigid body transform
Input:
rbt - (4,4) ndarray
Output:
v - (3,) array
w - (3,) array
"""
#YOUR CODE HERE
rbt = list(rbt)
print 'here'
R = np.array([[rbt[0][0], rbt[0][1], rbt[0][2]],
[rbt[1][0], rbt[1][1], rbt[1][2]],
[rbt[2][0], rbt[2][1], rbt[2][2]]])
# p = np.array([[rbt[0][3]], [rbt[1][3]], [rbt[2][3]]])
p = np.array([rbt[0][3], rbt[1][3], rbt[2][3]])
omega, theta = eqf.find_omega_theta(R)
v = eqf.find_v(omega, theta, p)
return (v, omega)
def align_zumys(zumy,zumy2,zumy_vel,zumy_vel2,ar_tags):
print 'aligning'
stop1,stop2,not_aligned = False,False,True
listener = tf.TransformListener()
rate = rospy.Rate(5)
for i in range(10):
try:
(trans, rot) = listener.lookupTransform(ar_tags[zumy], ar_tags[zumy2], rospy.Time(0))
(ob1_trans,ob1_rot) = listener.lookupTransform(ar_tags[zumy], ar_tags['obstacle'],rospy.Time(0))
(ob2_trans,ob2_rot) = listener.lookupTransform(ar_tags['obstacle'],ar_tags[zumy2], rospy.Time(0))
print 'TRANS: ' + str(trans[0]) + ',' + str(trans[1])
print 'OBSTACLE_ZUMY: ' + str(ob1_trans[0]+ob2_trans[0]) + ',' + str(ob1_trans[1]-ob2_trans[1])
if (abs(trans[1] - (ob1_trans[1]-ob2_trans[1])) < 0.3):
print 'not able to pass'
return False
except:
short_delay()
while not_aligned:
if stop1 == False:
print('first if')
try:
(trans, rot) = listener.lookupTransform(ar_tags[zumy2], ar_tags[zumy], rospy.Time(0))
except:
continue
(omega, theta) = eqf.quaternion_to_exp(np.array(rot))
v = eqf.find_v(omega,theta,trans)
twist = Twist()
twist.linear.x = v[0][0]
twist.angular.z = omega[2]
xdelta = 0.06
ydelta = 0.5
delta = .2
minspeed = 0.15
maxspeed = 0.3
m = 0.36
b = 0.07
angularfactor = 0.15
twist.angular.x = 0
twist.angular.y = 0
twist.linear.y = 0
twist.linear.z = 0
if abs(trans[0]) + abs(trans[1]) < delta:
print("State: STOP")
twist.linear.x = 0
twist.angular.z = 0
elif abs(trans[0]) < xdelta and trans[1] < 0:
print("State: Forward")
twist.angular.z = 0
twist.linear.x = 0
stop1 = True
else:
print("State: Turn")
print 'TRANS_X: ' + str(trans[0])
print 'TRANS_Y: ' + str(trans[1])
twist.linear.x = 0
factor = 0.2/abs(trans[0]*trans[1])
twist.angular.z = factor * abs(trans[0] * trans[1]);
zumy_vel2.publish(twist)
#repeat for other zumy so that they both turn to each other
if stop2 == False and stop1 == True:
print('second if')
try:
(trans, rot) = listener.lookupTransform(ar_tags[zumy], ar_tags[zumy2], rospy.Time(0))
except:
continue
(omega, theta) = eqf.quaternion_to_exp(np.array(rot))
v = eqf.find_v(omega,theta,trans)
twist2 = Twist()
twist2.linear.x = v[0][0]
twist2.angular.z = omega[2]
twist2.angular.x = 0
twist2.angular.y = 0
twist2.linear.y = 0
twist2.linear.z = 0
xdelta = 0.03
ydelta = 0.5
delta = .2
minspeed = 0.15
maxspeed = 0.3
m = 0.36
b = 0.07
angularfactor = 0.1
if abs(trans[0]) + abs(trans[1]) < delta:
print("State: STOP")
twist2.linear.x = 0
twist2.angular.z = 0
elif abs(trans[0]) < xdelta and trans[1] < 0:
print("State: Forward")
twist2.angular.z = 0
twist2.linear.x = 0
stop2 = True
else:
print("State: Turn")
print 'TRANS_X: ' + str(trans[0])
print 'TRANS_Y: ' + str(trans[1])
twist2.linear.x = 0
factor = 0.15/abs(trans[0]*trans[1])
twist2.angular.z = factor * abs(trans[0] * trans[1]);
zumy_vel.publish(twist2)
if stop1 is True and stop2 is True:
print 'zumys aligned'
not_aligned = False
break
rate.sleep()
return True
