def main(): init_pos = Vectors.V4D(0.65, 0.05, 0.03, 0) bound = Vectors.V4D(0.65, 0.55, 0.53, 0) edges = { 'init': init_pos, 'bound': bound } sock = create_socket() try: move.run(sock, 'left', left_arm, edges) except KeyboardInterrupt: sock.close()
def main(): init_pos = Vectors.V4D(0.656982770038, -0.752598021641, 0.0388609422173, 0) bound = Vectors.V4D(0.656982770038, -0.252598021641, 0.5388609422173, 0) edges = { 'init': init_pos, 'bound': bound } sock = create_socket() try: move.run(sock, 'right', right_arm, edges) except KeyboardInterrupt: sock.close()
def decide_increment(data): #TODO - decide based on data mutex.acquire() global direction global dz global last_data if last_data is None: last_data = data if (data is not None): wow = data['onset'] dwow = data['onset'] - last_data['onset'] if abs(dwow) > 0.01: dz = dwow / 10.0 print "dwow", dwow print "dz: ", dz inc = Vectors.V4D(0.0, direction * 0.05, dz, 0.0) mutex.release() last_data = data return inc
def run(sock, arm, pose_func, edges): current_p = edges['init'] i_bound = edges['init'] o_bound = edges['bound'] channel = Queue.Queue() # initial values increment = Vectors.V4D(0.00, 0.05, 0.00, 0); speed = 0.5 # limit the amount of movement calls lim = 1000 j = 0 mdata = None while (j < lim): j += 1 # socket call try: data, addr = sock.recvfrom(1024) mdata = recv_data(data) speed = mdata['energy'] def swap_inc(data): while time.time() < data['beat']: time.sleep(0.01) global direction mutex.acquire() direction *= -1 mutex.release() t = threading.Thread(target = swap_inc, args = (mdata,)) t.daemon = True t.start() except socket.error, e: None increment = decide_increment(mdata); current_p = current_p + increment current_p = clamp(current_p, i_bound, o_bound) pose_func(current_p) resp = ik_move(arm, pose_func(current_p)) if resp is not None: make_move(resp, arm, speed) print "--------" time.sleep(0.1)
def left_arm(pos): quaternion = Vectors.V4D(0.36, 0.88, -0.10, 0.26) position = Pose( position=Point( x=pos.x(), y=pos.y(), z=pos.z(), ), orientation=Quaternion( x=quaternion.x(), y=quaternion.y(), z=quaternion.z(), w=quaternion.w(), ), ) return position
def clamp(p, inner, outer): if p.x() > outer.x(): p = Vectors.V4D(outer.x(), p.y(), p.z(), p.w()) elif p.x() < inner.x(): p = Vectors.V4D(inner.x(), p.y(), p.z(), p.w()) if p.y() > outer.y(): p = Vectors.V4D(p.x(), outer.y(), p.z(), p.w()) elif p.y() < inner.y(): p = Vectors.V4D(p.x(), inner.y(), p.z(), p.w()) if p.z() > outer.z(): p = Vectors.V4D(p.x(), p.y(), outer.z(), p.w()) elif p.z() < inner.z(): p = Vectors.V4D(p.x(), p.y(), inner.z(), p.w()) if p.w() > outer.w(): p = Vectors.V4D(p.x(), p.y(), p.z(), outer.w()) elif p.w() < inner.w(): p = Vectors.V4D(p.x(), p.y(), p.z(), inner.w()) return p
def main(): userID = systemArgHandler() global limbRight global limbLeft rospy.init_node('lift_ik_prototype', anonymous=True) """ myps = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=pose2(init_pos), ) solve_move_trac(limbRight, myps) """ rate = rospy.Rate(5000.0) #main loop while not rospy.is_shutdown(): try: #defines the pose of the child from the parent buffUserLeft = tf_buffer.lookup_transform( 'cob_body_tracker/user_' + str(userID) + '/left_shoulder', 'cob_body_tracker/user_' + str(userID) + '/left_hand', rospy.Time()) buffUserRight = tf_buffer.lookup_transform( 'cob_body_tracker/user_' + str(userID) + '/right_shoulder', 'cob_body_tracker/user_' + str(userID) + '/right_hand', rospy.Time()) #get translations xL = buffUserLeft.transform.translation.x yL = buffUserLeft.transform.translation.y zL = buffUserLeft.transform.translation.z xR = buffUserRight.transform.translation.x yR = buffUserRight.transform.translation.y zR = buffUserRight.transform.translation.z #translate the translations #add offsets to each joint tranXL = -zL * 1.2 + 0.2 tranYL = xL * 1.5 - 0.28 tranZL = -yL * 1.1 + 0.40 tranXR = -zR * 1.2 + 0.2 tranYR = xR * 1.5 + 0.28 tranZR = -yR * 1.1 + 0.40 #tranRightX = -z * 1.2 #tranRightY = x * 1.2 #tranRightZ = -y * 0.9 #catch and continue after refresh rate except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() continue user_pos_left = Vectors.V4D( tranXL, #depth (z?) (inline depth 0.2) (limit 1.0) tranYL, #left-right (x?) (inline 0.28) (limit 1.0) tranZL, 0) #height (y?) (inline height 0.40) user_pos_right = Vectors.V4D( tranXR, #depth (z?) (inline depth 0.2) (limit 1.0) tranYR, #left-right (x?) (inline 0.28) (limit 1.0) tranZR, 0) #height (y?) (inline height 0.40) #user_rot = Vectors.V4D(math.pi/2, # ((math.atan2(y,x) + math.pi/2) % (math.pi * 2)) - 10*math.pi/180, # 0, 0) # rotation pointing the infront of Red user_rot = Vectors.V4D(0, math.pi / 2, 0, 0) rightArmPose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), #header=Header(stamp=rospy.Time.now(), frame_id=limbRight+'_arm_mount'), #header=Header(stamp=rospy.Time.now(), frame_id=limbRight+'_lower_shoulder'), pose=userPose(user_pos_left, user_rot), ) leftArmPose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), #header=Header(stamp=rospy.Time.now(), frame_id=limbRight+'_arm_mount'), #header=Header(stamp=rospy.Time.now(), frame_id=limbRight+'_lower_shoulder'), pose=userPose(user_pos_right, user_rot), ) #solve_move_trac(limbRight, leftArmPose) solve_move_trac(limbLeft, leftArmPose) #Refresh rate rate.sleep()
#orientation=Quaternion( # x=0, # y=1, # z=0, # w=0 # ), orientation=normalize( Quaternion(x=q_rot[0], y=q_rot[1], z=q_rot[2], w=q_rot[3]))) return pose_right #NOTE: right hand #index finger point left init_pos = Vectors.V4D( 0.6, #depth (z?) (inline depth 0.2) (limit 1.0) -0.60, #left-right (x?) (inline 0.28) (limit 1.0) 0.40, 0) #height (y?) (inline height 0.40) # Quaternion -> Quaternion def normalize(quat): quatNorm = math.sqrt(quat.x * quat.x + quat.y * quat.y + quat.z * quat.z + quat.w * quat.w) normQuat = Quaternion(quat.x / quatNorm, quat.y / quatNorm, quat.z / quatNorm, quat.w / quatNorm) return normQuat def systemArgHandler(): if len(sys.argv) == 2:
def main(): userID = systemArgHandler() global mylimb rospy.init_node('lift_ik_prototype', anonymous=True) """ myps = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=pose2(init_pos), ) solve_move_trac(mylimb, myps) """ rate = rospy.Rate(5000.0) #main loop while not rospy.is_shutdown(): try: #defines the pose of the child from the parent buffUserLeft = tf_buffer.lookup_transform( 'cob_body_tracker/user_' + str(userID) + '/left_shoulder', 'cob_body_tracker/user_' + str(userID) + '/left_hand', rospy.Time()) #get translations x = buffUserLeft.transform.translation.x y = buffUserLeft.transform.translation.y z = buffUserLeft.transform.translation.z #translate the translations tranRightX = -z * 1.2 + 0.2 tranRightY = x * 1.5 - 0.28 tranRightZ = -y * 1.1 + 0.40 #tranRightX = -z * 1.2 #tranRightY = x * 1.2 #tranRightZ = -y * 0.9 #catch and continue after refresh rate except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rate.sleep() continue user_pos = Vectors.V4D( tranRightX, #depth (z?) (inline depth 0.2) (limit 1.0) tranRightY, #left-right (x?) (inline 0.28) (limit 1.0) tranRightZ, 0) #height (y?) (inline height 0.40) #user_rot = Vectors.V4D(math.pi/2, # ((math.atan2(y,x) + math.pi/2) % (math.pi * 2)) - 10*math.pi/180, # 0, 0) user_rot = Vectors.V4D(0, math.pi / 2, 0, 0) leftArmPose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), #header=Header(stamp=rospy.Time.now(), frame_id=mylimb+'_arm_mount'), #header=Header(stamp=rospy.Time.now(), frame_id=mylimb+'_lower_shoulder'), pose=userPose(user_pos, user_rot), ) solve_move_trac(mylimb, leftArmPose) #Refresh rate rate.sleep()
Create goal Pose and call ik move ''' pose_right = Pose( position=Point( x=pos.x(), y=pos.y(), z=pos.z(), ), orientation=Quaternion(x=0, y=1, z=0, w=0), ) return pose_right #time.sleep(2) init_pos = Vectors.V4D(0.8, -0.47, 0.2, 0) bound = Vectors.V4D(0.656982770038, -0.252598021641, 0.5388609422173, 0) init_pos2 = Vectors.V4D(0.656982770038, -0.35, 0.1, 0) init_pos3 = Vectors.V4D(0.656982770038, -0.35, 0.4, 0) myps = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id='base'), pose=pose2(init_pos), ) print 'initialisation move...' resp = trac_ik_solve(mylimb, myps) if resp is not None: