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